Ocean three-dimensional combined observation system and trajectory tracking method
By designing a three-dimensional joint observation system of the ocean and a real-time optimization trajectory tracking method based on fuzzy control, the problem of low real-time and large trajectory tracking errors in the existing technology is solved, and the real-time monitoring of water and underwater in the marine ranch is realized and the real-time monitoring of water and underwater and underwater in the marine ranch is realized. Efficient tracking.
Patent Information
- Application Number
- CN202411165201.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-08-23
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2044-08-23
AI Technical Summary
The existing marine ranch monitoring technology cannot realize three-dimensional real-time monitoring of water and underwater, and the existing trajectory tracking methods cannot be jointly optimized and controlled when facing coupling relationships and mutually influencing systems, especially when the reference trajectory changes in large angles. , poor anti-interference ability, insufficient real-time and flexibility.
A three-dimensional joint observation system of marine 3D is designed, including surface mobile devices and underwater robots. It is connected through cables, and a real-time optimized trajectory tracking method based on fuzzy control is adopted to establish a kinematic model, perform linearization and discrete processing, and construct the state. Spatial expressions and output equations, deduce predictive equations, design a real-time optimized trajectory tracking controller based on fuzzy control, and adjust the horizontal and vertical velocities in real time to achieve trajectory tracking.
It realizes real-time three-dimensional observations on and underwater of the ocean, improves the real-time, accuracy and efficiency of trajectory tracking, reduces tracking errors, and improves the tracking accuracy and convergence speed when large angles of reference trajectory are changed.
Smart Images

Figure CN119037676A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of ocean observation, and in particular to an above-water and underwater ocean stereoscopic joint observation system and a trajectory tracking method. Background Art
[0002] Marine ranching is a new model of marine fishery production and an important measure to promote marine economic development and marine ecological civilization construction. Marine ranch monitoring, as an important part of the modern marine ranch construction system, is the basis for comprehensive and accurate understanding and research of marine ranches, involving resource and environmental monitoring, ecological carrying capacity assessment, production process control, safety forecasting and other key links. At present, marine ranch monitoring technology and equipment are still imperfect, mostly using buoys or underwater robots, which cannot achieve three-dimensional real-time monitoring above and below the water, seriously restricting the construction of modern marine ranches.
[0003] At present, most of the trajectory tracking methods for observation systems are trajectory tracking control for a single device. For example, PID, as a classic control method, is often used in trajectory tracking control systems. The control output is adjusted according to the deviation between the current trajectory of the system and the reference trajectory to achieve the desired state. The method is simple, easy to understand, and widely used. As a commonly used trajectory tracking method, the LQR method can directly solve the optimal controller parameters to ensure the global optimal solution of the system under given performance indicators. The method is relatively simple and easy to understand, and the input can be adjusted 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 respond to changes quickly, and has strong anti-interference ability.
[0004] However, the above trajectory tracking methods cannot jointly optimize and control systems with coupling relationships and mutual influence, nor can they accurately predict and constrain system models. In addition, most of the trajectories currently targeted are relatively smooth trajectories, and when faced with large angle changes in the reference trajectory, the real-time and accuracy of tracking cannot be guaranteed. Summary of the invention
[0005] Based on the above technical problems, the present invention proposes an ocean stereo joint observation system and a trajectory tracking method.
[0006] The technical solution adopted by the present invention is:
[0007] One of the purposes of the present invention is to provide a marine stereoscopic joint observation system, which includes a surface mobile device and an underwater robot, wherein the surface mobile device is connected to the underwater robot via a cable;
[0008] 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;
[0009] 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;
[0010] The surface mobile device and / or the underwater robot are also equipped with observation equipment, including sensors such as temperature, salinity and depth meters.
[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 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;
[0013] 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.
[0014] The second object of the present invention is to provide a trajectory tracking method for the joint observation system as described above, comprising the following steps: receiving GPS signals and shore station control instructions through a surface mobile device, tracking the trajectory according to a pre-set 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 to achieve trajectory tracking control of the ocean stereoscopic joint observation system.
[0015] Preferably, the surface mobile device performs trajectory tracking according to a preset reference trajectory, specifically comprising the following steps:
[0016] a. Establish the kinematic model of the surface mobile device;
[0017] 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;
[0018] 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;
[0019] d. The real-time optimized trajectory tracking controller based on fuzzy control obtained in step c is used to track the trajectory of the surface mobile device.
[0020] The beneficial technical effects of the present invention are:
[0021] 1. The present invention proposes a new type of above-water and underwater joint observation system which combines a surface mobile device and an underwater robot. The system can realize synchronous real-time stereoscopic observation of the ocean above and below the surface, and promote the construction of modern marine ranches.
[0022] 2. The present invention also proposes a trajectory tracking method for an ocean stereoscopic joint observation system, namely, a real-time optimized trajectory tracking method based on fuzzy control. This method takes the changes in the lateral and longitudinal velocities at each moment as the input of the fuzzy controller and the coefficients of the control quantity weight matrix in the objective function as the output, and adjusts the coefficients in real time for different trajectories, thereby increasing the real-time, accuracy and efficiency of trajectory tracking; and effectively solves the problems of excessive tracking errors caused by large changes in the heading angle of the reference trajectory and slow convergence of the actual trajectory oscillation during the trajectory tracking process. BRIEF DESCRIPTION OF THE DRAWINGS
[0023] Figure 1 It is a schematic diagram of the overall structural principle of the ocean stereoscopic joint observation system of the present invention;
[0024] Figure 2 It is a schematic diagram of the structural principle of the surface mobile device in the ocean stereoscopic joint observation system of the present invention;
[0025] Figure 3 for Figure 2 A schematic diagram of the arrangement of an implementation method of the first thruster group;
[0026] Figure 4 It is a schematic diagram of the structural principle of the underwater robot in the ocean stereoscopic joint observation system of the present invention;
[0027] Figure 5 It is a flow chart of the trajectory tracking method of the ocean stereo joint observation system of the present invention;
[0028] Figure 6 A schematic diagram of a coordinate system for establishing a kinematic model in the trajectory tracking method of the present invention;
[0029] Figure 7 It is a flow chart of real-time optimization coefficients in the trajectory tracking method of the present invention;
[0030] Figure 8 This is a sine wave trajectory tracking effect diagram when verifying the trajectory tracking method of the present invention;
[0031] Fig. 9 This is a rectangular trajectory tracking effect diagram when verifying the trajectory tracking method of the present invention;
[0032] Fig.10 for Fig. 9 A partial enlarged view of
[0033] Fig.11 This is a diagram showing the effect of the oblique square wave trajectory tracking when verifying the trajectory tracking method of the present invention;
[0034] Fig.12 for Fig.11 A partial enlarged view of .
[0035] In the figure: 1-surface mobile device, 2-underwater robot, 3-cable, 4-first buoy, 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-electronic cabin, 14-second buoy, 15-weight-bearing steel sheet, 16-second horizontal thruster, 17-vertical thruster, 18-LED lighting, 19-electronic cabin fixing ring, 20-umbilical cable hook, 21-pad. DETAILED DESCRIPTION
[0036] In view of the problem that the current ocean observation mostly uses single equipment such as buoys or underwater robots, and cannot realize the real-time stereoscopic monitoring above and below the water, the present invention proposes an ocean stereoscopic joint observation system, which can realize the synchronous real-time stereoscopic observation of the ocean above and below the water. At the same time, the present invention also provides a trajectory tracking method for the ocean stereoscopic joint observation system. This is mainly because the existing trajectory tracking methods are not fully applicable in the ocean field, exposing problems such as large tracking errors, poor anti-interference ability, poor real-time and flexibility. The trajectory tracking method proposed by the present invention, that is, 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 above-water and underwater joint observation system, but also ensure the accuracy and real-time performance of tracking when the reference trajectory changes over a large range.
[0037] The present invention will be further described below in conjunction with the accompanying drawings and specific implementations.
[0038] like Figure 1 As shown, a marine stereoscopic joint observation system includes a surface mobile device 1 and an underwater robot 2, and the surface mobile device 1 is connected to the underwater robot 2 through a cable 3.
[0039] like Figure 2As shown, the 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 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 on the top of the antenna support rod 7.
[0040] Specifically, the first floating body 4 is in a flat cylindrical shape, and the first propeller group 5 includes four first horizontal propellers 501, which are arranged at intervals along the circumference of the bottom surface of the first floating body. Figure 3 As shown. A thruster guard plate 8 is arranged below the first thruster group. The thruster guard plate 8 is in a circular shape and is connected to the first buoy 4 through a vertical support rod. A bracket 9 is arranged on the top of the first buoy 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 arranged in total, and a GPS antenna 10 is arranged on the top of one antenna support rod, and a data transmission radio antenna 11 is arranged on the top of the other antenna support rod. The antenna support rod can be made of glass fiber material.
[0041] like Figure 4 As shown, the underwater robot 2 includes a support frame 12, on which a second thruster group is arranged, and an electronic cabin 13 is also arranged on the support frame, and the electronic cabin 13 is connected to the electric control box 6 through a cable 3. A second float 14 is arranged on the upper part of the support frame 12, and a load-bearing steel sheet 15 is arranged on 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 arranged to jointly realize the travel, steering and underwater positioning of the underwater robot. An LED lighting lamp 18 is also arranged on the support frame. The electronic cabin 13 is connected to the electronic cabin fixing ring 19. An umbilical cable hook 20 is also arranged on the top of the support frame 12.
[0042] The surface mobile device and the underwater robot are also equipped with observation sensors such as temperature, salinity and depth meters, which are not shown in the figure.
[0043] In summary, the present invention designs a new above-water and underwater joint observation system or operation system, which consists of two parts, one is a surface mobile device on the water, which can also be called an unmanned surface mobile relay (USMR), and the other is an underwater robot (ROV) underwater. The unmanned surface mobile relay is composed of a GPS module, a data transmission radio module, an electric control electrical box, a float and four thrusters. The four thrusters of the USMR meet the lateral and longitudinal axis symmetry to give it better maneuverability. The control equipment and batteries are placed in the electric control electrical box. The float is used to ensure stability and better load-bearing capacity when moving on the water surface. It is also equipped with a data transmission radio with long-distance transmission capability to receive and send signals. The ROV is composed of a float, an electric control unit and eight thrusters. The USMR and the ROV are connected by a cable. The GPS signal received by the USMR is combined with underwater positioning to obtain the precise geographic coordinates of the ROV, thereby 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, equipped with various sensors for detection.
[0044] The present invention also provides a trajectory tracking method for the above-mentioned ocean stereoscopic joint observation system, which receives GPS signals and shore station control instructions through a surface mobile device, performs high-precision trajectory tracking according to a pre-set reference trajectory, and then transmits the position information and control instructions to an underwater robot through a cable in real time, so that the underwater robot can accurately track the surface mobile device, thereby achieving trajectory tracking control of the ocean stereoscopic joint observation system.
[0045] It can be seen that the trajectory tracking method based on the above-mentioned ocean stereo joint observation system only needs to perform precise trajectory tracking control on the surface mobile device on the water.
[0046] like Figure 5 As shown, the surface mobile device performs trajectory tracking according to a preset reference trajectory, specifically including the following steps:
[0047] a. Establish the kinematic model of the water surface mobile device.
[0048] like Figure 6 As shown, first establish the inertial reference coordinate system {O E X E Y E} and the body reference coordinate system {O B X B Y B}.
[0049] 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. The three-degree-of-freedom water surface mobile device model in the Newton-Lagrangian form is established as follows:
[0050]
[0051] 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(v) is the damping force matrix, τ is the power input matrix, τ w is the environmental interference matrix.
[0052] Without considering the influence of high-order nonlinear damping, the kinematic model of the surface mobile device is as follows:
[0053]
[0054] x,y, are defined in the inertial coordinate system, which are the position and heading angle of USMR. is the differential of the ordinate, is the differential of the horizontal axis, is the differential of the heading angle, u, ν, r are the longitudinal velocity, lateral velocity and yaw angular velocity in the body coordinate system.
[0055] b. Using the kinematic model of the surface mobile device obtained in step a, establish the state space equation, and perform linearization and discretization on the state space equation in turn to construct a new state space expression and output equation.
[0056] The specific steps include:
[0057] b1. Establish the state space equation.
[0058] In order to better distinguish the various components in the control quantity, the above longitudinal velocity is changed 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 system is established as follows:
[0059]
[0060] And the following nonlinear equation is obtained:
[0061]
[0062] is the differential of the system state quantity, the system state quantity They represent the position and heading angle in the two-dimensional coordinate system respectively. 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.
[0063] b2. Linearization of state space equations.
[0064] The joint observation system is a complex nonlinear system. In order to simplify the control problem and improve efficiency, the nonlinear equation obtained in step b1 is linearized to establish the error state equation:
[0065]
[0066] 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.
[0067]
[0068] The linearized state space equation is:
[0069]
[0070] m is the n is the
[0071] b3. Discretization of state space equations.
[0072] After the linearization of the model in step b2, the linear state space equation is discretized using the forward Euler method to obtain the linear discrete state space equation:
[0073]
[0074] Representative Matrix Representative Matrix
[0075] In order to more intuitively observe the change of the control quantity in the later constraint part, a new state quantity needs to be constructed. The new state quantity needs to introduce the change of the control quantity at the previous moment.
[0076]
[0077] From the linear discrete state space equation and the new state quantity ζ(k), we can get:
[0078]
[0079] I nu is the identity matrix of the number of control quantities,
[0080] The new state space expression is constructed as follows:
[0081]
[0082] The output equation is:
[0083]
[0084] I nx It is the identity matrix of the number of state quantities and the third-order identity matrix. This definition is intended to be passed through K = [I nx 0] this matrix to make the output equation contain only state variables but not control variables. In addition, the output equation can be changed flexibly through I nx This unit matrix is used to control the state quantity you want to output.
[0085] c. Using 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 a real-time optimization trajectory tracking controller based on fuzzy control.
[0086] c1. Construct prediction equation;
[0087] The prediction equation is obtained by deducing the new state space expression and output equation established in step b3;
[0088] First, the new state space expression is derived:
[0089]
[0090] N p is the prediction time domain, N c is the control time domain, satisfying N p ≥N c .
[0091] Similarly, the output equation is derived as follows:
[0092]
[0093] 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:
[0094] Z=ρζ(k)+σΔU
[0095] 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 control quantity change.
[0096] 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 prediction result within is the output of the output equation, thus completing the prediction part.
[0097] c2. Design and optimize the objective function;
[0098] set up Q and R are N c The identity matrix of numbers, is the Kronecker product, an operation symbol in linear algebra; the designed objective function is as follows:
[0099]
[0100] Simplifying, 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 formula can be written as follows:
[0103]
[0104] And add constraints to the objective function, the control amount and control amount increment have the following forms:
[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 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:
[0109]
[0110] Such constraints are added because the control quantity is subject to some restrictions, 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. The second is to optimize the performance indicators. By adding constraints and adjusting the control input, the effect of trajectory tracking is optimized 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. Real-time optimization trajectory tracking controller based on fuzzy control or fuzzy controller design.
[0113] In the objective function, Q q and R r The matrix is the weight coefficient matrix, which plays different roles in trajectory tracking. q is the state weight matrix, which can reflect the importance of different state variables, Q q The larger the value of the element in the matrix, the more the system pays attention to the trajectory tracking accuracy, and it can converge to the reference trajectory faster to reduce the tracking error. r is the control quantity weight matrix, which can reflect the importance of different control quantities. r The larger the value of the element 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, such as Figure 7 shown.
[0114] More specifically, the method further comprises the following steps:
[0115] 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 quantity is the real-time optimized matrix coefficient, and the fuzzy quantities of seven membership functions are defined, which are {NB, NM, NS, ZO, PS, PM, PB}, and the membership functions are set to Gaussian membership function and S-type membership function respectively.
[0116] The Gaussian membership function satisfies:
[0117]
[0118] The Gaussian membership function is determined by two parameters σ and c, σ is generally positive, and c determines the center of the curve.
[0119] The S-type membership function satisfies:
[0120]
[0121] 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.
[0122] 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:
[0123]
[0124] P=m×n is the total number of rules. Based on the above expression, a fuzzy control rule table can be formulated.
[0125] Finally, defuzzification is required, and the centroid method is generally used for defuzzification. First, the weighted average is calculated, which is achieved by taking the weighted average of the membership function 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 area covered by the membership function, which ranges from [μ min ,μ max ].
[0128] The normalization coefficient is then calculated to ensure that the output value is within the appropriate range. The normalization function is usually defined as the sum of the membership functions of the fuzzy output quantity:
[0129]
[0130] Finally, calculate the defuzzified result:
[0131]
[0132] μ f represents the output after defuzzification. The fuzzy control process is basically completed. r The matrix is rewritten for optimization:
[0133]
[0134] μ 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.
[0135] d. The real-time optimized trajectory tracking controller based on fuzzy control obtained in step c is used to track the trajectory of the surface mobile device.
[0136] The trajectory tracking method of the present invention increases the real-time, accuracy and efficiency of trajectory tracking, and effectively solves the problems of excessive tracking error caused by large angle changes of the reference trajectory heading angle and slow convergence speed of the actual trajectory oscillation during the trajectory tracking process.
[0137] The trajectory tracking method of the present invention is verified accordingly below.
[0138] Specifically, the method proposed in the present invention is simulated and verified through three trajectories: a sinusoidal wave trajectory, a rectangular trajectory and an oblique square wave trajectory. It is illustrated that the method proposed in the present invention is not only applicable to the case where the reference trajectory changes in a small range in real time, but also has a good tracking effect when facing sudden large-angle changes in the same and reverse directions.
[0139] in, Figure 8 This is a diagram showing the effect of sinusoidal wave trajectory tracking when verifying the trajectory tracking method of the present invention. Figure 9-10 This is a diagram showing the rectangular trajectory tracking effect when verifying the trajectory tracking method of the present invention. Figure 11-Figure 12 This is a diagram showing the effect of oblique square wave trajectory tracking when verifying the trajectory tracking method of the present invention.
[0140] The rectangular trajectory tracking performance indicators are shown in Table 1.
[0141] Table 1
[0142]
[0143] The performance indicators of the oblique square 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 it takes for the actual trajectory to converge to the reference trajectory. Figure 8-Figure 12 As can be seen from Tables 1 and 2, for sinusoidal wave trajectories, the tracking effects before and after real-time optimization are very good. When the reference trajectories such as rectangular and oblique square waves suddenly change at a large angle, the tracking effect after real-time optimization is significantly improved compared with that before optimization, which improves the accuracy and real-time performance of trajectory tracking.
Claims
1. A three-dimensional ocean joint observation system, characterized in that: It includes a surface mobile device and an underwater robot, wherein the surface mobile device is 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.
2. 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 joint observation system according to claim 1 or 2, characterized in that The following steps are involved: The surface mobile device receives GPS signals and shore station control commands, tracks the trajectory according to the pre-set reference trajectory, and then transmits the position information and control commands to the underwater robot in real time through cables, so that the underwater robot can track the surface mobile device and achieve trajectory tracking control of the ocean stereo joint observation system.
4. The trajectory tracking method of the ocean stereo joint observation system according to claim 3, characterized in that: 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. The real-time optimized trajectory tracking controller based on fuzzy control obtained in step c is used to track the trajectory of the surface mobile device.
5. The trajectory tracking method of the ocean stereo joint observation system according to claim 4, 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(v) 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.
6. The trajectory tracking method of the ocean stereo joint observation system according to claim 5, 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.
7. The trajectory tracking method of the ocean stereo joint observation system according to claim 6, 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 8. The trajectory tracking method of the ocean stereo joint observation system according to claim 7, 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.
9. The trajectory tracking method of the ocean stereo joint observation system according to claim 8, 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 time 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 control 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.
10. The trajectory tracking method of the ocean stereo joint observation system according to claim 9, characterized in that: Step c3 also includes the following steps: 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 ΔV 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 factor 15 μf(k) , R f (k) represents the final R matrix at time k.
Citation Information
Patent Citations
Underwater vehicle control system and motion control method
CN107499476A
Integrated acoustic positioning and tracking control method for autonomous underwater vehicle
CN112947068A
Underwater large underwater vehicle water surface observation system and observation method
CN116055689A
Autonomous underwater robot trajectory tracking control method considering quantitative state feedback
CN116107323A
Water surface and underwater trajectory tracking control method and device for cross-medium aircraft
CN117555351A
Cited By
Wheeled robot tracking control method and system facing network delay and interference
CN120428718A
