Dynamic positioning control method for under-actuated unmanned surface vehicle

Abstract

The invention relates to a dynamic positioning control method for an underactuated water surface unmanned ship, and the method comprises the steps: outputting the real-time motion state information of the unmanned ship through Beidou second generation, integrated navigation and AIS equipment carried by the unmanned ship at the current moment, and carrying out the filtering through a nonlinear estimation filter. Meanwhile, a nonlinear PID environment compensator is designed to compensate marine environment interference received by the underactuated unmanned surface vehicle; constructing a dynamic positioning controller based on a model predictive control algorithm (MPC) to receive the filtered real-time motion state information of the unmanned ship and predict the motion state of the unmanned ship at the future moment to obtain an optimal command rudder angle, and outputting the optimal command rudder angle to an unmanned ship motion system; and the unmanned ship motion system continuously feeds back the actual value to the controller to perform rolling optimization. According to the invention, dynamic positioning of the underactuated marine survey unmanned ship under complex sea conditions can be realized, and accurate marine environment survey can be carried out on a sea area with a specified coordinate.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned surface vehicle control, and in particular to a dynamic positioning control method for an underactuated unmanned surface vehicle. Background Art

[0002] Unmanned surface vehicles are an increasingly widely used intelligent surface mission platform. They are used in the field of marine environment measurement and mapping because of their unmanned, intelligent, low cost, long endurance, good economic efficiency, convenient loading of operating loads, and ability to replace manual operations in dangerous waters. Figure 1 The ocean survey unmanned boat shown plays an increasingly important role in the fields of ocean environment survey, seabed topography survey, and marine resource development, and has broad application prospects.

[0003] The surface unmanned boat is equipped with oceanographic measuring instruments: multi-beam sonar, single-beam sonar, sound velocity profiler, shallow layer profiler, temperature-salinity-depth instrument, ocean temperature chain, gravimeter, magnetometer, side-scan sonar, etc., to conduct three-dimensional ocean environment measurement on the surface, in the air, and on the seabed. The ocean environment measurement mainly includes: measurement of marine geological elements, measurement of marine water elements, and measurement of seawater acoustic environment elements. Among them, the measurement of marine geological elements mainly includes: marine topography measurement, marine water depth measurement, seabed landform measurement, marine surface bottom measurement, marine shallow profile measurement, marine magnetic measurement, marine gravity measurement, tidal correction measurement, sound velocity profile measurement, etc. It is mainly used to compile various nautical charts and make digital drawings. The measurement of marine water elements mainly includes seawater density measurement, seawater temperature measurement, seawater salinity measurement, current and wave measurement, tidal measurement, seawater transparency detection, etc. The measurement of underwater acoustic environment elements includes: underwater acoustic propagation, background noise, ocean reverberation, seabed reflection, bottom sound velocity, sound velocity profile, etc., to grasp the ocean noise level and noise background.

[0004] When the ocean measurement unmanned boat uses the winch to retract the temperature chain and sound velocity profiler to measure the seawater temperature, salinity, and sound velocity profile curves at different depths in the ocean, the unmanned boat needs to be fixed at a certain operating point for long-term measurements. When the ocean measurement unmanned boat is used to measure and map topography, it is necessary to measure the seawater temperature and seawater sound velocity profile at different depths to achieve data correction for the multi-beam sonar measurement of topography. When measuring parameters such as seawater temperature, sound velocity profile, and salinity, the unmanned boat is required to conduct long-term measurements at designated measurement points, and dynamic positioning technology that meets the functional requirements of ocean measurement unmanned boats is urgently needed.

[0005] Ocean measurement unmanned boats are disturbed by marine environmental forces such as wind, waves, and currents on the sea surface, and they are under-actuated, making it difficult to achieve fixed-point operation requirements. Therefore, there is an urgent need for dynamic positioning control technology that meets marine operation requirements and is suitable for under-actuated ocean measurement unmanned boats. Summary of the invention

[0006] The purpose of the present invention is to overcome the shortcomings of the prior art and propose a dynamic positioning control method for an under-actuated unmanned surface vessel, which can realize dynamic positioning of an under-actuated ocean measurement unmanned vessel under complex sea conditions and achieve accurate ocean environment measurement in a specified coordinate sea area.

[0007] The present invention solves the technical problem by adopting the following technical solutions:

[0008] A dynamic positioning control method for an underactuated unmanned surface vehicle comprises the following steps:

[0009] Step 1: At the current moment, the real-time motion state information of the unmanned boat is output by the Beidou II, inertial integrated navigation and AIS equipment carried by the unmanned boat, and filtered by a nonlinear estimation filter to obtain accurate motion state information of the unmanned boat: position information, heading information, speed information, etc.

[0010] Step 2: construct a model prediction controller to receive the filtered real-time unmanned boat motion state information and predict the unmanned boat motion state at the future moment, obtain the optimal command rudder angle, and output it to the unmanned boat motion system;

[0011] Step 3: The unmanned boat motion system will continue to feed back the actual value to the controller for rolling optimization.

[0012] Moreover, the real-time unmanned boat motion status information in step 1 includes position information, speed information and heading information.

[0013] Moreover, the nonlinear estimation filter in step 1 is:

[0014]

[0015] Among them, M is the inertia matrix of the unmanned boat, R(ψ) is the conversion matrix, D is the linear hydrodynamic matrix of the unmanned boat, m represents the mass of the ship, I z represents the moment of inertia, It represents the additional mass caused by the acceleration of the fluid dynamics in the three directions of surge, sway and pitch, and is defined as a negative number. It is the additional mass caused by the coupling of sway and pitch. is the estimated matrix of the unmanned boat operation state, A ω is a constant, is the derivative of the low-frequency position and yaw angle vector, is the lateral velocity derivative of the unmanned boat, is the three-dimensional state vector of the unmanned boat, T is a three-dimensional diagonal matrix containing time constants, R T(ψ) is the transformation matrix of R(ψ), Derivative of the three-dimensional state vector of the unmanned boat, is the rank transformation of the inertia matrix of the unmanned boat, is the inertia matrix variable of the unmanned boat. is the estimation error; W1∈R 6×3 ,W2,W3,W4∈R 3×3 is the gain matrix, and the nonlinear PID is used as an environmental compensator to compensate for the interference of ocean wind, waves or current environment when the underactuated surface unmanned vehicle is in a designated operating area in the ocean:

[0016]

[0017] τ=-K i R T (ψ)(θ-θ d )-K d v p R T (ψ)(κ-κ d )

[0018] in, is the estimated matrix of the operating state of the unmanned boat with marine environment interference, π d is the marine environment interference coefficient matrix, θ d is the expected state variable; K p , K i , K d are the three control parameters of the PID controller: proportional, integral and differential. The nonlinear PID environment compensates for the interference of the ocean wind, waves or current environment when the underactuated surface unmanned boat is in a designated operating area in the ocean, outputs the control input command τ of the surface unmanned boat, and improves the anti-interference ability of the underactuated surface unmanned boat.

[0019] Moreover, the specific practical method of step 2 is: construct a model prediction controller to receive real-time unmanned boat motion state information and predict the unmanned boat motion state at future moments, and at the same time feed back the error between the predicted value and the reference value to the rolling optimization part, construct an optimization function for the unmanned boat position deviation, and finally optimize the function under the constraints of state variables and control inputs to find the optimal solution, obtain the optimal command rudder angle, and output it to the unmanned boat motion system.

[0020] Moreover, the model prediction controller is constructed by using the unmanned boat motion mathematical model, wherein the unmanned boat motion mathematical model is:

[0021]

[0022] Among them, in the motion coordinates of the surface unmanned boat, the x-axis is specified to point to the true north, and the angle between the bow and stern centerline of the boat and the x-axis is the bow angle. express. and V c are the flow direction and flow velocity, u r is the forward speed against water, v r is the water lateral movement speed, the hydration speed The components of the speed of the unmanned surface vehicle along the x-axis and y-axis in the oxyz coordinate system are u and v, respectively. u is the forward speed relative to the ground, v is the lateral speed relative to the ground, the angular velocity of the bow rotating around the z-axis is r, and the total speed relative to the ground is V = (u 2 +v 2 ) 1 / 2 , drift angle β = arctan (v / u), δ is the rudder angle. r is the command rudder angle, K E is the servo control gain, T E is the steering gear time constant, m is the mass of the unmanned surface vehicle, m x and m y is the additional mass, X H , Y H and N H is the viscous fluid dynamics acting on the hull, X P , Y P and N P is the propeller force in the X, Y, and Z directions of the coordinate system, X W , Y W and N W is the wind force in the X, Y, and Z directions of the coordinate system, X Wave , Y Wave and N Wave is the wave force in the X, Y, and Z directions of the coordinate system, I ZZ is the moment of inertia of the surface unmanned vehicle around the vertical axis, J ZZ is the additional moment of inertia, X R , Y R and N R is the rudder force in the X, Y, and Z directions of the coordinate system, t R is the rudder resistance reduction, a H is the ratio of the additional lateral force on the hull caused by steering to the lateral force on the rudder, x H is the distance from the center of action of the lateral force induced by steering to the center of gravity of the unmanned surface boat, F N is the rudder positive pressure.

[0023] The calculation method of rudder torque is:

[0024]

[0025] Among them, t R is the rudder resistance reduction, a H is the ratio of the additional lateral force on the hull caused by steering to the lateral force on the rudder, xH is the distance from the center of action of the lateral force induced by steering to the center of gravity of the unmanned surface boat, F N is the rudder positive pressure.

[0026] Discretize and predict each state of the unmanned boat motion mathematical model:

[0027]

[0028] in, is the motion state of the unmanned boat at the future k+1 time, is the motion state of the unmanned boat at time k. c is the prediction sampling time, which is the time interval between two consecutive prediction values. and They are the discretization of each differential equation in the model predictive controller;

[0029] and They are the discretization of the differential equations in the mathematical model of unmanned boat motion:

[0030]

[0031] Among them, u(k) is the speed of the unmanned boat at time k, ψ(k) is the heading angle of the unmanned boat at time k, v(k) is the lateral speed of the unmanned boat at time k, r(k) is the angular velocity of the bow of the unmanned boat rotating around the z-axis at time k, and F u For 11 For m 11 is, τ(k) is, d uE (k) is, X is, Y is, F is r For u is, δ(k) is the rudder angle at time k, δ r (k) is the command rudder angle at time k. Is the compensation value for the unknown item. Is the compensation value for the unknown item. Only the lateral displacement is considered for path tracking. From the current k moment to the future N P The output at each moment is predicted.

[0032] Moreover, in step 2, the error between the predicted value and the reference value is fed back to the rolling optimization part, and the specific implementation method of constructing the optimization function of the position deviation of the unmanned boat is: the model predictive controller only considers the lateral displacement to realize dynamic positioning, and the future N P The output at each moment is predicted:

[0033]

[0034] According to the predicted results and the reference lateral displacement yd , calculate the position prediction error where j = 1, 2, ..., N P :

[0035]

[0036] According to the prediction error, construct the optimization function:

[0037]

[0038] Where Q is the weight matrix, which is obtained by solving the QP quadratic form or under the constraint δ min ≤δ≤δ max Solve the equation Calculate the optimal control law.

[0039] Moreover, in step 2, optimizing the function to find the optimal solution under the constraints of the state variables and the control input to obtain the optimal command rudder angle includes the following steps:

[0040] Step 1: Initialize the parameters of the flock optimization algorithm and divide the entire flock into several sub-groups. Each sub-group consists of a rooster, several hens and chicks. The number of sub-groups is determined by the number of roosters.

[0041] Step 2: Evaluate the fitness value of each individual in the chicken flock, and divide the population into roosters, hens and chicks according to the fitness value of each chicken. The roosters are those with good fitness values, the chicks are those with poor fitness values, and the rest are hens. Set t = 1;

[0042] Step 3: Determine whether t is divisible by σ. If it is divisible, update the hierarchy of the chicken flock; if not, update the hierarchy among roosters, hens, and chickens respectively;

[0043] Step 4: Update the hierarchy between roosters, hens, and chicks;

[0044] Step 5: Update the fitness values ​​of roosters, hens, and chickens, and calculate and update the global optimal individuals of the population;

[0045] Step 6: Compare whether t is greater than the maximum number of iterations. If so, output the optimal value; if not, return to step (3).

[0046] The advantages and positive effects of the present invention are:

[0047] The present invention uses the Beidou II, combined navigation and AIS equipment carried by the unmanned boat to output real-time unmanned boat motion state information at the current moment, and filters it through a nonlinear estimation filter; constructs a model prediction controller to receive the filtered real-time unmanned boat motion state information and predict the unmanned boat motion state at future moments, obtains the optimal command rudder angle, and outputs it to the unmanned boat motion system; the unmanned boat motion system will continue to feed back the actual value to the controller for rolling optimization. The present invention can realize the dynamic positioning of the underactuated ocean measurement unmanned boat in complex sea conditions, and achieve accurate ocean environment measurement in a specified coordinate sea area. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 An underactuated ocean surveying type unmanned surface boat used in the present invention;

[0049] Figure 2 It is a structural diagram of the dynamic positioning controller of the present invention;

[0050] Figure 3 It is a flow chart of the chicken flock optimization algorithm of the present invention. DETAILED DESCRIPTION

[0051] The present invention is further described in detail below with reference to the accompanying drawings.

[0052] like Figure 1 The figure shows an underactuated unmanned surface boat. This type of unmanned boat is equipped with multi-beam sonar, single-beam echo sounder, ADCP, CTD, sound velocity profiler, magnetometer, meteorological instrument and other oceanographic measuring instruments. It can autonomously navigate to measure and map ocean topography, ocean magnetism, ocean hydrological and meteorological information, etc. The unmanned boat needs to perform fixed-point measurement when measuring ocean sound velocity profile and seabed topography. Here, a dynamic positioning control method for an underactuated unmanned boat based on ocean measurement and mapping operations is designed. Figure 2 The figure shows the principle block diagram of the dynamic positioning control method of the underactuated unmanned boat. The operator remotely sets the position coordinates of the unmanned boat's fixed-point operation. After receiving the command, the unmanned boat goes to the designated position coordinate point to perform fixed-point measurement and mapping operations. Affected by the wind, waves, and currents at sea, the surface unmanned boat will deviate from the designated coordinate position. The inertial combined navigation instrument continuously measures the position information of the unmanned boat in real time and compares it with the set position information, and inputs the position error to the dynamic positioning controller based on model predictive control. After algorithm processing, the control command is output to the diesel engine (throttle command, steering command) to the surface unmanned boat, thereby controlling the speed and heading of the unmanned boat to achieve automatic adjustment of the position information.

[0053] Combination Figure 1 , Figure 2 , Figure 3 As shown, a dynamic positioning control method for an underactuated unmanned surface vehicle comprises the following steps:

[0054] Step 1: At the current moment, the real-time unmanned boat motion state information is output through the Beidou II, integrated navigation and AIS equipment carried by the unmanned boat, and filtered through a nonlinear estimation filter. The real-time unmanned boat motion state information includes position information, speed information and heading information.

[0055] The signal measured by the sensor contains the information of the movement of the surface unmanned boat and some noise information mixed together. At the same time, the movement of the surface unmanned boat is a composite movement of high-frequency movement and low-frequency movement, and the high-frequency movement of the surface unmanned boat is a periodic oscillation that does not change the average position of the surface unmanned boat. Therefore, for the actual controller control process, the high-frequency signal is actually a kind of interference, and a filter is needed to filter out the high-frequency signal before control.

[0056] The low-frequency model of the underactuated ocean measurement unmanned boat is:

[0057]

[0058] Among them, E v is a diagonal matrix, ω v is zero mean white noise.

[0059] The motion data of the underactuated ocean measurement unmanned boat measured by the sensor of the underactuated ocean measurement unmanned boat is generally composed of the superposition of the low-frequency signal of the surface unmanned boat, the high-frequency signal and some measurement noise, so they must be taken into consideration in the modeling process, so the measurement model of the system can be obtained:

[0060] y=M+M G +K y

[0061] Where, Μ represents the low-frequency position and the heading angle vector, Μ G represents the high frequency position and the heading angle vector, K y represents the measurement noise vector.

[0062] According to the characteristics of the underactuated ocean measurement unmanned boat system model, a nonlinear estimation filter is constructed:

[0063]

[0064] Among them, M is the inertia matrix of the unmanned boat, R(ψ) is the conversion matrix, D is the linear hydrodynamic matrix of the unmanned boat, m represents the mass of the ship, I z represents the moment of inertia, It represents the additional mass caused by the acceleration of the fluid dynamics in the three directions of surge, sway and pitch, and is defined as a negative number. It is the additional mass caused by the coupling of sway and pitch. is the estimated matrix of the unmanned boat operation state, A ω is a constant, is the derivative of the low-frequency position and yaw angle vector, is the lateral velocity derivative of the unmanned boat, is the three-dimensional state vector of the unmanned boat, T is a three-dimensional diagonal matrix containing time constants, R T (ψ) is the transformation matrix of R(ψ), Derivative of the three-dimensional state vector of the unmanned boat, is the rank transformation of the inertia matrix of the unmanned boat, is the inertia matrix variable of the unmanned boat. is the estimation error; W1∈R 6×3 ,W2,W3,W4∈R 3×3 is the gain matrix, and the nonlinear PID is used as an environmental compensator to compensate for the interference of ocean wind, waves or current environment when the underactuated surface unmanned vehicle is in a designated operating area in the ocean:

[0065]

[0066] τ=-K i R T (ψ)(θ-θ d )-K d v p R T (ψ)(κ-κ d )

[0067] in, is the estimated matrix of the operating state of the unmanned boat with marine environment interference, π d is the marine environment interference coefficient matrix, θ d is the expected state variable; K p , K i , K d are the three control parameters of the PID controller: proportional, integral and differential. The nonlinear PID environment compensates for the interference of the ocean wind, waves or current environment when the underactuated surface unmanned boat is in a designated operating area in the ocean, outputs the control input command τ of the surface unmanned boat, and improves the anti-interference ability of the underactuated surface unmanned boat.

[0068] Step 2: Build a model prediction controller to receive the filtered real-time unmanned boat motion state information and predict the unmanned boat motion state at future moments, obtain the optimal command rudder angle, and output it to the unmanned boat motion system.

[0069] The specific practical method of step 2 is: construct a model prediction controller to receive real-time unmanned boat motion state information and predict the unmanned boat motion state at future moments. At the same time, the error between the predicted value and the reference value is fed back to the rolling optimization part, and the optimization function of the unmanned boat position deviation is constructed. Finally, under the constraints of state variables and control inputs, the optimization function is optimized to find the optimal solution, the optimal command rudder angle is obtained, and it is output to the unmanned boat motion system.

[0070] The model predictive controller is constructed through the mathematical model of the unmanned boat motion, where the mathematical model of the unmanned boat motion is:

[0071]

[0072] Among them, in the motion coordinates of the surface unmanned boat, the x-axis is specified to point to the true north, and the angle between the bow and stern centerline of the boat and the x-axis is the bow angle. express. and V c are the flow direction and flow velocity, u r is the forward speed against water, v r is the water lateral movement speed, the hydration speed The components of the speed of the unmanned surface vehicle along the x-axis and y-axis in the oxyz coordinate system are u and v, respectively. u is the forward speed relative to the ground, v is the lateral speed relative to the ground, the angular velocity of the bow rotating around the z-axis is r, and the total speed relative to the ground is V = (u 2 +v 2 ) 1 / 2 , drift angle β = arctan (v / u), δ is the rudder angle. r is the command rudder angle, K E is the servo control gain, T E is the steering gear time constant, m is the mass of the unmanned surface vehicle, m x and m y is the additional mass, X H , Y H and N H is the viscous fluid dynamics acting on the hull, X P , Y P and N P is the propeller force in the X, Y, and Z directions of the coordinate system, X W , Y W and N W is the wind force in the X, Y, and Z directions of the coordinate system, X Wave , Y Wave and N Wave is the wave force in the X, Y, and Z directions of the coordinate system, I ZZ is the moment of inertia of the surface unmanned vehicle around the vertical axis, J ZZ is the additional moment of inertia, X R , Y R and NR is the rudder force in the X, Y, and Z directions of the coordinate system, t R is the rudder resistance reduction, a H is the ratio of the additional lateral force on the hull caused by steering to the lateral force on the rudder, x H is the distance from the center of action of the lateral force induced by steering to the center of gravity of the unmanned surface boat, F N is the rudder positive pressure.

[0073] The calculation method of rudder torque is:

[0074]

[0075] Where, tR is the rudder resistance reduction, a H is the ratio of the additional lateral force on the hull caused by steering to the lateral force on the rudder, x H is the distance from the center of action of the lateral force induced by steering to the center of gravity of the unmanned surface boat, F N is the rudder positive pressure.

[0076] Discretize and predict each state of the unmanned boat motion mathematical model:

[0077]

[0078] Among them, among them, is the motion state of the unmanned boat at the future k+1 time, is the motion state of the unmanned boat at time k. c is the prediction sampling time, which is the time interval between two consecutive prediction values. and They are the discretization of each differential equation in the model predictive controller.

[0079]

[0080] Among them, u(k) is the speed of the unmanned boat at time k, ψ(k) is the heading angle of the unmanned boat at time k, v(k) is the lateral speed of the unmanned boat at time k, r(k) is the angular velocity of the bow of the unmanned boat rotating around the z-axis at time k, and F u For 11 For m 11 is, τ(k) is, d uE (k) is, X is, Y is, F is r For u is, δ(k) is the rudder angle at time k, δ r (k) is the command rudder angle at time k. Is the compensation value for the unknown item. Is the compensation value for the unknown item. Only the lateral displacement is considered for path tracking. From the current k moment to the future N PThe output at each moment is predicted.

[0081] Moreover, in step 2, the error between the predicted value and the reference value is fed back to the rolling optimization part, and the specific implementation method of constructing the optimization function of the position deviation of the unmanned boat is: the model predictive controller only considers the lateral displacement to realize dynamic positioning, and the future N P The output at each moment is predicted:

[0082]

[0083] According to the predicted results and the reference lateral displacement y d , calculate the position prediction error where j = 1, 2, ..., N P :

[0084]

[0085] According to the prediction error, construct the optimization function:

[0086]

[0087] Where Q is the weight matrix, which is obtained by solving the QP quadratic form or under the constraint δ min ≤δ≤δ max Solve the equation Calculate the optimal control law. Since the model predictive control algorithm uses computer online memory calculation, all intelligent online optimizations are performed to calculate the control law, and the analytical expression of the control law cannot be directly obtained. For the multiple interference terms contained in the optimization function, the chicken flock optimization algorithm is used to solve them.

[0088] The chicken flock optimization algorithm is based on the simulation of chicken flock behavior. The roles of roosters, hens and chicks in the chicken flock are determined by the fitness value in the chicken flock. The chicken with the best fitness value is selected as a rooster, the chicken with the worst fitness value is a chick, and the rest of the chickens in the chicken flock are regarded as hens. The rooster with the best fitness value has priority over the rooster with the lower fitness value to obtain food. The higher the fitness value, the larger the attack search range:

[0089] x i,j (t+1)=x i,j (t)(1+Randn(0,σ 2 ))

[0090]

[0091] Among them, x i,j (t) represents the position coordinate of individual i at the tth iteration, Randn(0,σ 2 ) is the mean value of 0 and the standard deviation of σ 2A Gaussian distribution function. i is the fitness value of individual i. ε is a sufficiently small positive number to prevent The denominator of is 0. k is the number of any individual hen, and k≠i.

[0092] The hen searches for food with the rooster, and will randomly steal good food found by other chickens. In the process of stealing food, the other chickens will inhibit her. In the competition for food, the strong hen has an advantage over the docile hen. The position of the hen is updated as follows:

[0093] x i,j (t+1)=x i,j (t)+S1*Randn(x r1,j (t)-x i,j (t))+S1*Randn(x r2,j (t)-x i,j (t))

[0094] S1=exp((f i -f r1 ) / (abs(f i )+ε))

[0095] S2=exp((f r2 -f i )

[0096] Where r1 is the number of the rooster that is the mate of the i-th hen, r2 is the number of any randomly selected rooster or hen, and r1≠r2, and Randn is a random number in [0,1].

[0097] The smaller the hen's fitness value, the closer S1 is to 1, and the smaller the position gap with the rooster is. The more dominant hen is, the more likely it is to get food. The chicks look for food around the hen, and the position of the chicks is updated to:

[0098] x i,j (t+1)=x i,j (t)+FL*(x m,j (t)-x i,j (t))

[0099] In the formula, x m,j (t) represents the position of the mother of the i-th chick at the t-th iteration, a random number in FL*()∈[0,2].

[0100] like Figure 3 As shown, the swarm optimization algorithm includes the following steps:

[0101] Step 1: Initialize the parameters of the flock optimization algorithm and divide the entire flock into several sub-groups. Each sub-group consists of a rooster, several hens and chicks. The number of sub-groups is determined by the number of roosters.

[0102] Step 2: Evaluate the fitness value of each individual in the chicken flock, and divide the population into roosters, hens and chicks according to the fitness value of each chicken. The roosters are those with good fitness values, the chicks are those with poor fitness values, and the rest are hens. Set t = 1;

[0103] Step 3: Determine whether t is divisible by σ. If it is divisible, update the hierarchy of the chicken flock; if not, update the hierarchy among roosters, hens, and chickens respectively;

[0104] Step 4: Update the hierarchy between roosters, hens, and chicks;

[0105] Step 5: Update the fitness values ​​of roosters, hens, and chickens, and calculate and update the global optimal individuals of the population;

[0106] Step 6: Compare whether t is greater than the maximum number of iterations. If so, output the optimal value; if not, return to step (3).

[0107] Step 3: The unmanned boat motion system will continue to feed back the actual value to the controller for rolling optimization.

[0108] It should be emphasized that the embodiments described in the present invention are illustrative rather than restrictive. Therefore, the present invention includes but is not limited to the embodiments described in the specific implementation manner. Any other implementation manners derived by those skilled in the art based on the technical solution of the present invention also fall within the scope of protection of the present invention.

Claims

1. A dynamic positioning control method for an underactuated unmanned surface vehicle, characterized in that: The following steps are involved: Step 1: At the current moment, the real-time motion status information of the unmanned boat is output through the Beidou II, integrated navigation and AIS equipment carried by the unmanned boat, and filtered through a nonlinear estimation filter; Step 2: construct a model prediction controller to receive the filtered real-time unmanned boat motion state information and predict the unmanned boat motion state at the future moment, obtain the optimal command rudder angle, and output it to the unmanned boat motion system; Step 3: The unmanned boat motion system will continue to feed back the actual value to the controller for rolling optimization.

2. A dynamic positioning control method for an underactuated unmanned surface vehicle according to claim 1, characterized in that: The real-time unmanned boat motion status information in step 1 includes position information, speed information and heading information.

3. The dynamic positioning control method for an underactuated unmanned surface vehicle according to claim 1, characterized in that: The nonlinear estimation filter in step 1 is: Among them, M is the inertia matrix of the unmanned boat, R(ψ) is the conversion matrix, D is the linear hydrodynamic matrix of the unmanned boat, m represents the mass of the ship, I z represents the moment of inertia, It represents the additional mass caused by the acceleration of the fluid dynamics in the three directions of surge, sway and pitch, and is defined as a negative number. is the additional mass caused by the coupling of sway and pitch, is the estimated matrix of the unmanned boat operation state, A ω is a constant, is the derivative of the low-frequency position and yaw angle vector, is the lateral velocity derivative of the unmanned boat, is the three-dimensional state vector of the unmanned boat, T is a three-dimensional diagonal matrix containing time constants, R T (ψ) is the transformation matrix of R(ψ), Derivative of the three-dimensional state vector of the unmanned boat, is the rank transformation of the inertia matrix of the unmanned boat, is the inertia matrix variable of the unmanned boat, is the estimation error; W1∈R 6×3 ,W2,W3,W4∈R 3×3 is the gain matrix, and the nonlinear PID is used as an environmental compensator to compensate for the interference of ocean wind, waves or current environment when the underactuated surface unmanned vehicle is in a designated operating area in the ocean: τ=-K i R T (ψ)(θ-θ d )-K d vK p R T (ψ)(k-k) d ) in, is the estimated matrix of the operating state of the unmanned boat with marine environment interference, π d is the marine environment interference coefficient matrix, θ d is the expected state variable; K p , K i , K d are the three control parameters of the PID controller, namely the proportion, integration and differentiation. The nonlinear PID environment compensates for the interference of the ocean wind, waves or flow environment when the under-actuated surface unmanned vessel is in a designated operating area in the ocean, and outputs the control input command τ of the surface unmanned vessel to improve the anti-interference ability of the under-actuated surface unmanned vessel.

4. The method for dynamic positioning control of an underactuated unmanned surface vehicle according to claim 1, characterized in that: The specific implementation method of step 2 is: construct a model prediction controller to receive real-time unmanned boat motion state information and predict the unmanned boat motion state at future moments, and at the same time feed back the error between the predicted value and the reference value to the rolling optimization part, construct an optimization function for the unmanned boat position deviation, and finally optimize the function under the constraints of state variables and control inputs to find the optimal solution, obtain the optimal command rudder angle, and output it to the unmanned boat motion system.

5. A dynamic positioning control method for an underactuated unmanned surface vehicle according to claim 4, characterized in that: The model prediction controller is constructed by using the unmanned boat motion mathematical model, wherein the unmanned boat motion mathematical model is: Among them, in the motion coordinates of the surface unmanned boat, the x-axis is specified to point to the true north, and the angle between the bow and stern centerline of the boat and the x-axis is the bow angle. express, and V c are the flow direction and flow velocity, u r is the forward speed against water, v r is the water lateral movement speed, the hydration speed The components of the speed of the unmanned surface vehicle along the x-axis and y-axis in the oxyz coordinate system are u and v, respectively. u is the forward speed relative to the ground, v is the lateral speed relative to the ground, the angular velocity of the bow rotating around the z-axis is r, and the total speed relative to the ground is V = (u 2 +v 2 ) 1 / 2 , drift angle β = arctan (v / u), δ is the rudder angle, δ r is the command rudder angle, K E is the servo control gain, T E is the steering gear time constant, m is the mass of the surface unmanned boat, m x and m y is the additional mass, X H , Y H and N H is the viscous fluid dynamics acting on the hull, X P , Y P and N P is the propeller force in the X, Y, and Z directions of the coordinate system, X W , Y W and N W is the wind force in the X, Y, and Z directions of the coordinate system, X Wave , Y Wave and N Wave is the wave force in the X, Y, and Z directions of the coordinate system, I ZZ is the moment of inertia of the surface unmanned vehicle around the vertical axis, J ZZ is the additional moment of inertia, X R , Y R and N R is the rudder force in the X, Y, and Z directions of the coordinate system, t R is the rudder resistance reduction, a H is the ratio of the additional lateral force on the hull caused by steering to the lateral force on the rudder, x H is the distance from the center of action of the lateral force induced by steering to the center of gravity of the unmanned surface boat, F N is the rudder positive pressure, The calculation method of rudder torque is: Among them, t R is the rudder resistance reduction, a H is the ratio of the additional lateral force on the hull caused by steering to the lateral force on the rudder, x H is the distance from the center of action of the lateral force induced by steering to the center of gravity of the unmanned surface boat, F N is the rudder positive pressure, Discretize and predict each state of the unmanned boat motion mathematical model: in, is the motion state of the unmanned boat at the future k+1 time, is the motion state of the unmanned boat at time k, T c is the prediction sampling time, which is the time interval between two consecutive prediction values. and They are the discretization of each differential equation in the model predictive controller; and The calculation method is: in, is the compensation value for the unknown item, is the compensation value for the unknown item, only considering the lateral displacement for path tracking, from the current k moment to the future N P The output at each moment is predicted.

6. A dynamic positioning control method for an underactuated unmanned surface vehicle according to claim 5, characterized in that: In step 2, the error between the predicted value and the reference value is fed back to the rolling optimization part. The specific implementation method of constructing the optimization function of the position deviation of the unmanned boat is: the model predictive controller only considers the lateral displacement to realize dynamic positioning, and the future N P The output at each moment is predicted: According to the predicted results and the reference lateral displacement y d , calculate the position prediction error where j = 1, 2, ..., N P : According to the prediction error, construct the optimization function: Where Q is the weight matrix, which is obtained by solving the QP quadratic form or under the constraint δ min ≤δ≤δ max Solve the equation Calculate the optimal control law.

7. A dynamic positioning control method for an underactuated unmanned surface vehicle according to claim 6, characterized in that: In step 2, optimizing the function to find the optimal solution under the constraints of the state variables and the control input to obtain the optimal command rudder angle includes the following steps: Step 1: Initialize the parameters of the flock optimization algorithm and divide the entire flock into several sub-groups. Each sub-group consists of a rooster, several hens and chicks. The number of sub-groups is determined by the number of roosters. Step 2: Evaluate the fitness value of each individual in the chicken flock, and divide the population into roosters, hens and chicks according to the fitness value of each chicken. The roosters are those with good fitness values, the chicks are those with poor fitness values, and the rest are hens. Set t = 1; Step 3: Determine whether t is divisible by σ. If it is divisible, update the hierarchy of the chicken flock; if not, update the hierarchy among roosters, hens, and chickens respectively; Step 4: Update the hierarchy between roosters, hens, and chicks; Step 5: Update the fitness values ​​of roosters, hens, and chickens, and calculate and update the global optimal individuals of the population; Step 6: Compare whether t is greater than the maximum number of iterations. If so, output the optimal value; if not, return to step (3).

Images