Two-stage hybrid propulsion amphibious unmanned platform active disturbance rejection control method and system

Abstract

The invention provides a two-stage hybrid propulsion amphibious unmanned platform active disturbance rejection control method and system, and belongs to the field of amphibious unmanned platforms. The objective of the invention is to solve the problems that the expected speed or expected depth is difficult to track and the data processing speed is slow when an existing active-disturbance-rejection control method is applied to control of a multi-habitat unmanned platform. Compared with a traditional control method, the method has the advantages that the requirement for the precision of a controlled object is not high, the anti-interference capacity is high, meanwhile, control parameters of the active-disturbance-rejection controller can be adjusted, nonlinear error feedback control is changed into a linear form, and the control precision is high. The linear expansion state observer is adopted to well estimate external disturbance of the amphibious unmanned platform and internal disturbance of the system, compensation is given to the control quantity, the parameter setting difficulty is reduced, adaptability and robustness are higher, and operation is easy and convenient; a sliding window filtering algorithm is adopted to filter sensor data, noise interference of signals is filtered out, data fluctuation is smoother, and the data processing speed is increased.

Description

Technical Field

[0001] The present invention relates to the technical field of amphibious unmanned platforms, and in particular to a two-stage hybrid propulsion amphibious unmanned platform self-disturbance control method and system. Background Art

[0002] With the development of the world economy and the need for national defense construction, it is a general trend to vigorously develop the marine economy, exploit marine resources and strengthen marine armament construction. However, with the deepening of marine development and the further demand for armament construction, there is a need for an amphibious unmanned carrier platform that can work in an environment that humans cannot reach or is harmful, can carry a variety of functional equipment, can sail in the water, crawl on the bottom of the water and travel on land to complete tasks such as marine environment observation, seabed topography mapping, underwater rescue, unmanned island landing operations, military exploration, etc. At present, most of the amphibious unmanned carrier platforms of various countries are in the development and testing state and have not yet been tested in actual combat. They cannot meet the needs of amphibious operations in terms of flexibility, autonomy, coordination and other aspects, and there is still a lot of room for technical development; bionic and wheel-paddle amphibious vehicles have poor obstacle crossing performance, easy to slip, poor scalability, environmental adaptability, and poor controllability.

[0003] The multi-functional unmanned carrier platform should have the advantages of scalability, versatility, good maneuverability, fast response speed, and reasonable price. The present invention is based on the unmanned submarine as the prototype and practicality and versatility as the principle. Design a multi-functional unmanned carrier platform with a certain carrying capacity and data collection capability, capable of unmanned autonomous planning movement and good human-computer interaction function. The operator can grasp the status of the multi-functional unmanned carrier platform in real time and intervene in the task planning in time to improve the success rate of the task.

[0004] Active disturbance rejection control (ADRC) technology is an emerging control theory that aims to solve the problem of disturbance suppression in traditional control systems. ADRC technology was proposed by Han Jingqing of the Chinese Academy of Sciences, and then gradually developed and widely used. ADRC technology has strong anti-interference ability and robustness, and can effectively deal with various disturbances inside and outside the system. It is one of the current research hotspots in the field of control. Traditional control systems often find it difficult to effectively deal with various disturbances when facing complex systems, especially disturbances in nonlinear and time-varying systems. Traditional linear control methods such as PID controllers often require precise mathematical models and parameter adjustments. In actual engineering, it is often difficult to obtain an accurate mathematical model of the system. Therefore, it is easily affected by external disturbances, resulting in a decrease in system performance. In order to overcome this problem, active disturbance rejection control technology came into being.

[0005] The core idea of ​​the ADRC technology is to estimate the disturbance in the system by introducing an observer, treating the disturbance as part of the system, and introducing a compensation mechanism in the controller so that the system can actively suppress the disturbance. Compared with traditional control methods, the ADRC technology does not require an accurate system model, but only requires measuring the input and output of the system to achieve stable control of the system, and has strong adaptability and robustness.

[0006] In the development process of ADRC technology, researchers have continuously improved and optimized its theoretical framework and algorithms, and proposed a series of improved methods and technologies. For example, the model-based ADRC method has conducted in-depth research on system modeling and parameter adjustment, improving the stability and performance of the system; the ADRC method based on adaptive identification uses adaptive identification technology to update the system model in real time, improving the system's ability to adapt to parameter changes; the multi-model-based ADRC method (MM-ADRC) has achieved remarkable results in multi-model switching control and is suitable for the control of complex multi-modal systems. However, when the existing ADRC methods are applied to the control of multi-habitat unmanned platforms, there are problems such as difficulty in tracking the expected speed or expected depth and slow data processing speed. Summary of the invention

[0007] The technical problems to be solved by the present invention are:

[0008] In order to solve the problem that when the existing active disturbance rejection control method is applied to the control of multi-habitat unmanned platforms, it is difficult to track the expected speed or expected depth and the data processing speed is slow.

[0009] The present invention adopts the following technical solutions to solve the above technical problems:

[0010] The present invention provides an automatic disturbance rejection control method for an amphibious unmanned platform with two-stage hybrid propulsion, comprising the following steps:

[0011] S100, establish a dynamic model of an amphibious unmanned platform in water,

[0012] The hydraulic power and the control force of the actuator are expressed as follows:

[0013]

[0014] Among them, M A is the additional mass matrix, C A (V) is the hydrodynamic Coriolis matrix, D(V) is the hydrodynamic damping matrix, g(Θ) is the gravity buoyancy matrix, and τ is the control force matrix of the actuator;

[0015] in,

[0016]

[0017]

[0018]

[0019] The hydrodynamic damping is expressed as follows:

[0020] D(V)V=D l V+D u V+D nl (V)V

[0021] Among them, D l is the linear damping matrix, D u is the damping matrix that increases with increasing longitudinal velocity, D nl (V) is the nonlinear damping matrix;

[0022] in,

[0023]

[0024]

[0025]

[0026] Assume that the mass of the amphibious unmanned platform immersed in water is W = mg, where g is the acceleration of gravity; the buoyancy is B = ρg▽, where ρ is the water density and ▽ is the volume of the amphibious unmanned platform immersed in water; Assume that the gravity and buoyancy are equal, that is, W = B, and the center of buoyancy of the amphibious unmanned platform is r B =[x B ,y B ,z B ] T , and y B =0, then we have the following vector:

[0027]

[0028] The amphibious unmanned platform is an underactuated amphibious unmanned platform, and the longitudinal thrust, bow moment and pitch moment are expressed in vector form as follows:

[0029] τ=[τ u 000 τ q τ r ] T

[0030] Among them, τ u is the longitudinal thrust, τ q is the pitch moment, τ r is the bow turning moment;

[0031] By simplifying the six-degree-of-freedom mathematical model, the horizontal plane mathematical model of the amphibious unmanned platform can be obtained, which can be expressed in the form of a differential equation group as follows:

[0032]

[0033] Where A = -d 22 v+(d 26 -uc 26 )r,B=(d 62 -uc 62 )vd 66 r+τ r ,and,

[0034]

[0035] S200, establishing an auto-disturbance rejection controller for a multi-habitat unmanned platform, the auto-disturbance rejection controller comprising a tracking differentiator, a linear extended state observer and a linear error feedback module, the tracking differentiator is used to arrange the transition process to smooth the sudden change part of the input signal; the extended state observer tracks the influence of the unknown part of the model and the external unknown disturbance by establishing an extended state quantity; the control quantity is given through the linear error feedback module to compensate for the disturbance, so that the multi-habitat unmanned platform can adjust its heading in real time, and output the current actual heading to re-introduce it into the linear extended state observer to continue the cyclic control.

[0036] Furthermore, in step S200, it includes:

[0037] S210, the tracking differentiator receives the set desired heading angle information ψ d , calculate the approximate value x of the desired heading angle according to the kinematic model of step S100 1 And the differential signal x of the approximate heading angle 2 ;

[0038] S220, the linear extended state observer receives the actual heading angle information ψ and the input control variable u of the multi-habitat unmanned platform, and calculates the estimated values ​​of the actual heading of the multi-habitat unmanned platform and its first-order differential, which are respectively expressed as z 1 and z 2 , and calculate the total disturbance z of the multi-habitat unmanned platform 3 ;

[0039] S230, calculate x obtained by tracking differentiator 1 and the observed variable z 1 The deviation e 1 And the corresponding differential signal x 2 With z 2 The deviation e 2 , get e 1 =x1 -z 1 , e 2 =x 2 -z 2 ;

[0040] S240, the linear error feedback module according to the error e 1 and error e 2 Obtain the control component and use the estimated value of the unknown disturbance z 3 Perform disturbance compensation on the control component to obtain the control quantity u of the multi-habitat unmanned platform;

[0041] S250, the amphibious unmanned platform adjusts its heading in real time after receiving the control value u, and outputs the current actual heading to the linear expansion state observer to continue the cyclic control.

[0042] Furthermore, in step S210, a sliding window filtering algorithm is used to filter the data sampled by the sensor in the navigation and positioning system, including:

[0043] Set a sampling window with a length of M and a filtering window with a length of N. After the sensor starts sampling, if the data, i.e., the number of sample points, is less than M, then the data in the sampling window is accumulated and the average value is calculated as the result after filtering; if the number of data sample points is greater than M, then the sampling data before time M is discarded, and the data in the sampling window is bubble sorted, and the maximum and minimum values ​​are removed. The remaining M-2 data form a filtering window, satisfying M-2=N. The data in the filtering window is accumulated and the average value is calculated to obtain the filtered result.

[0044] Furthermore, when the amphibious unmanned platform is performing depth-fixed navigation, if the difference between the sum of the actual depth and the actual height minus the expected depth is less than 0.5 meters, the amphibious unmanned carrier platform changes from depth-fixed navigation to 0.5-meter altitude-fixed navigation.

[0045] Furthermore, the amphibious unmanned platform realizes the bow control of the amphibious unmanned platform by controlling the vertical rudder during high-speed navigation, including:

[0046] In the rudder mode, incremental PID control is used. The error between the actual heading angle and the expected value is used as the input of the PID controller, and the output is the corresponding rudder angle deflection. The expression is as follows:

[0047] Δθ(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2))

[0048] Among them, e(k) is the heading angle error at the current moment, e(k-1) is the heading angle error at the previous moment, e(k-2) is the heading angle error at the moment before that, and K p is the proportional gain term, K i is the integral gain term, K d is the differential gain term.

[0049] Furthermore, the in-situ steering of the amphibious unmanned platform is achieved through the main propulsion differential, using incremental PID control. The error between the actual heading angle and the expected value is used as the input of the PID controller, and the output is the resultant force, which is expressed as follows:

[0050] ΔF(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2))

[0051] Where, e(k) is the heading angle error at the current moment, e(k-1) is the heading angle error at the previous moment, and e(k-2) is the heading angle error at the moment before that; ΔF = |F 1 |+|F 2 |, where F 1 is the thrust distribution of the left main thruster (501), F 2 is the thrust distribution of the right main thruster (502), and F 1 +F 2 =0.

[0052] Furthermore, the speed control of the multi-functional unmanned platform includes speed maintenance and maneuvering speed change. Incremental PID control is used, and the error between the actual speed and the expected value is used as the input of the PID controller. The output is the resultant force, and its expression is as follows:

[0053] ΔF(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2))

[0054] Where e(k) is the speed error at the current moment, e(k-1) is the speed error at the previous moment, and e(k-2) is the speed error at the moment before that; ΔF = |F 1 |+|F 2 |, F 1 is the thrust distribution of the left main thruster (501), F 2 It is the thrust size distributed by the right main thruster (502).

[0055] Furthermore, in the horizontal rudder mode, the depth control of the amphibious unmanned platform is realized through the vertical thruster, horizontal rudder and main thruster, and an incremental PID control algorithm is adopted, with the depth error as the input of the PID controller and the horizontal rudder angle as the output;

[0056] Combined with the line of sight navigation method, the 10m ahead of the multi-habitat unmanned platform’s own X direction position is taken as the expected X d Direction position, the desired depth position is Z d , and the displacement error equation is obtained:

[0057] x e =xx d ,z e =zz d

[0058] The desired trim angle is:

[0059] θ d =tan -1 ((zz d ) / (xx d )).

[0060] The present invention provides a two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control system. The system has a program module corresponding to the above steps, and executes the steps in the above two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control method when running.

[0061] The present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of a two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control method when called by a processor.

[0062] Compared with the prior art, the present invention has the following beneficial effects:

[0063] The present invention discloses an automatic anti-disturbance control method and system for an amphibious multi-purpose unmanned water and land platform. Compared with the traditional control method, the method has low requirements on the accuracy of the control object and strong anti-disturbance ability. At the same time, the control parameters of the automatic anti-disturbance controller can be adjusted, and the nonlinear error feedback control is changed into a linear form. The linear extended state observer can well estimate the external interference of the amphibious multi-purpose unmanned water and land platform and the internal disturbance of the system to compensate for it at the control quantity, thereby reducing the difficulty of parameter setting, and having stronger adaptability and robustness and simple operation. The sliding window filtering algorithm is used to filter the sensor data, thereby filtering out the noise interference of the signal, making the data fluctuation smoother and accelerating the data processing speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 This is a front view of the multi-habitat unmanned platform in an embodiment of the present invention;

[0065] Figure 2 A top view of a multi-habitat unmanned platform according to an embodiment of the present invention;

[0066] Figure 3 The structure of the stern direction of the multi-habitat unmanned platform in an embodiment of the present invention is schematically shown;

[0067] Figure 4 Schematic diagram of the structure of the multi-habitat unmanned platform anti-disturbance control system in an embodiment of the present invention;

[0068] Figure 5 This is a parameter setting diagram of an amphibious unmanned platform in an embodiment of the present invention;

[0069] Figure 6 It is a schematic diagram of the thrust distribution of the main thrust differential left turn control in an embodiment of the present invention;

[0070] Figure 7 It is a schematic diagram of thrust distribution for speed control of a multi-habitat unmanned platform in an embodiment of the present invention;

[0071] Figure 8 Schematic diagram of the depth control thrust distribution of the multi-habitat unmanned platform in an embodiment of the present invention.

[0072] Description of reference numerals:

[0073] 2. Track chassis; 3. Battery compartment; 4. Electronic compartment; 5. External frame; 6. Buoyancy block; 201. Driving wheel; 202. Load wheel; 203. Guide wheel; 204. Track; 501. Left main thruster; 502. Right main thruster; 503. Stern vertical thruster; 504. Stern horizontal thruster; 505. Bow vertical thruster; 506. Bow horizontal thruster; 508. Doppler sensor. DETAILED DESCRIPTION

[0074] In the description of the present invention, it should be noted that the terminology nouns in the various embodiments, such as "up", "down", "front", "back", "left", "right", etc., which indicate directions, are only for simplifying the description of the positional relationship based on the drawings in the specification, and do not mean that the referred elements and devices must be operated according to the specific directions and defined operations and methods and structures in the specification. Such directional nouns do not constitute a limitation on the present invention.

[0075] In the description of the present invention, it should be noted that the terms "first", "second", and "third" mentioned in the embodiments of the present invention are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Therefore, the features defined as "first", "second", and "third" may explicitly or implicitly include one or more of the features.

[0076] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, specific embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0077] Specific implementation plan 1: Combine Figures 1 to 4 As shown, the present invention provides a two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control method, comprising the following steps:

[0078] S100, establish the dynamic model of amphibious unmanned platform in water, including:

[0079] The hydraulic power and the control force of the actuator are expressed as follows:

[0080]

[0081] Among them, M A is the additional mass matrix, C A (V) is the hydrodynamic Coriolis matrix, D(V) is the hydrodynamic damping matrix, g(Θ) is the gravity buoyancy matrix, and τ is the control force matrix of the actuator;

[0082] in,

[0083]

[0084]

[0085]

[0086] Since establishing the six-degree-of-freedom precise hydrodynamic resistance and torque matrix of the amphibious unmanned platform is a very challenging task, it is almost impossible to find the hydrodynamic coefficients that meet all speeds for the amphibious unmanned platform that works in a large speed range. Moreover, when the longitudinal speed of the amphibious unmanned platform is relatively large, the nonlinearity of the hydrodynamic force will be very obvious. At the same time, the coupling between the various degrees of freedom of the amphibious unmanned platform will also be enhanced. The hydrodynamic coefficients used in the present invention are measured by empirical formulas and water tank experiments, and their correctness can be guaranteed when the working speed of the amphibious unmanned platform is 2-4 meters per second.

[0087] Combined with the above discussion, for amphibious unmanned platforms, the hydrodynamic damping can be expressed as follows:

[0088] D(V)V=D l V+D u V+D nl (V)V

[0089] Among them, D l is the linear damping matrix, D u is the damping matrix that increases with increasing longitudinal velocity, Dnl (V) is the nonlinear damping matrix;

[0090] in,

[0091]

[0092]

[0093]

[0094] Assume that the mass of the amphibious unmanned platform immersed in water is W = mg, where g is the acceleration of gravity; the buoyancy is B = ρg▽, where ρ is the water density and ▽ is the volume of the amphibious unmanned platform immersed in water; Assume that the gravity and buoyancy are equal, that is, W = B, and the center of buoyancy of the amphibious unmanned platform is r B =[x B ,y B ,z B ] T , and y B =0, then we have the following vector:

[0095]

[0096] The amphibious unmanned platform of the present invention is an underactuated amphibious unmanned platform, and its actuator can only provide longitudinal thrust, bow moment and pitch moment, which are expressed in the form of vectors as follows:

[0097] τ=[τ u 000 τ q τ r ] T

[0098] Among them, τ u is the longitudinal thrust, τ q is the pitch moment, τ r is the bow turning moment;

[0099] By simplifying the six-degree-of-freedom mathematical model, the horizontal plane mathematical model of the amphibious unmanned platform can be obtained, which can be expressed in the form of a differential equation group as follows:

[0100]

[0101] Where A = -d 22 v+(d 26 -uc 26 )r,B=(d 62 -uc 62 )vd 66 r+τ r ,and,

[0102]

[0103] The parameters of the amphibious unmanned platform involved in the present invention are as follows: Figure 5 As shown;

[0104] S200, establishing an amphibious unmanned platform anti-disturbance controller, the anti-disturbance controller includes a tracking differentiator, a linear extended state observer and a linear error feedback module, the tracking differentiator is used to arrange the transition process, which can make the sudden change part of the input signal smooth, alleviate the contradiction between rapidity and overshoot in PID control technology, and also extract the differential signal of the input signal to avoid noise amplification; the extended state observer is used to solve the comprehensive influence of the unknown part of the model and the external unknown disturbance on the control object, and an extended state quantity is established to track the influence of the unknown part of the model and the external unknown disturbance; then the control quantity is given to compensate for these disturbances, and the control object is changed into an ordinary integral series control object. The purpose of establishing the extended state observer is to observe the extended state variables to estimate the unknown disturbance and the unmodeled part of the control object, realize the feedback linearization of the dynamic system, and change the control object into an integral series type;

[0105] Specifically include:

[0106] S210, the tracking differentiator receives the set desired heading angle information ψ d , calculate the approximate value of the desired heading angle x 1 , and the differential signal x of the approximate heading angle 2 ;

[0107] S220, the linear extended state observer receives the actual heading angle information ψ and the input control variable u of the amphibious unmanned platform, and calculates the estimated values ​​of the actual heading of the amphibious unmanned platform and its first-order differential, which are respectively expressed as z 1 and z 2 , and calculate the total disturbance z of the amphibious unmanned platform 3 , the total disturbance z 3 It includes external environmental forces called external disturbances and internal disturbances of the system caused by inaccurate models or changes in model parameters;

[0108] S230, calculate x obtained by tracking differentiator 1 and the observed variable z 1 The deviation e 1 And the corresponding differential signal x 2 With z 2 The deviation e 2 , get e 1 =x 1 -z 1 , e 2 =x 2 -z 2 ;

[0109] S240, the linear error feedback module according to the error e 1 and error e 2 Obtain the control component and use the estimated value of the unknown disturbance z 3 Perform disturbance compensation on the control component to obtain the control quantity u of the amphibious unmanned platform;

[0110] S250, the amphibious unmanned platform adjusts its heading in real time after receiving the control value u, and outputs the current actual heading to the linear expansion state observer to continue the cyclic control.

[0111] Preferably, in step S210, a sliding window filtering algorithm is used to filter the data sampled by the sensor in the navigation and positioning system, and a sampling window with a length of M and a filtering window with a length of N are set. After the sensor starts sampling, if the number of data sample points does not fill the sampling window, that is, the number of sample points is less than M, the data in the sampling window is accumulated and the average value is calculated as the filtered result; if the data sample points have filled the sampling window, that is, the number of sample points is greater than M, the sampling data before the M moment is discarded, and only the M sampling data obtained in the most recent M moments are considered to form the sampling window, and then the data in the sampling window is bubble sorted, and the maximum and minimum values ​​therein are removed. The remaining M-2 data form the filtering window, that is, M-2=N is satisfied, and the data in the filtering window is accumulated and the average value is calculated to obtain the filtered result; the sensor data processed by the sliding window filtering algorithm is filtered out of noise interference, and the data fluctuation is more stable and smooth; because each time a new data is sampled, the data before M moments is discarded, so a new average value can be calculated for each sampling, which speeds up the data processing speed.

[0112] Specific implementation plan 2: When the multi-functional unmanned platform is performing depth-fixing navigation, it can adaptively switch between the depth-fixing and altitude-fixing modes to ensure the safety of the multi-functional unmanned carrier platform; specifically, it includes:

[0113] When conducting depth-fixed navigation, when the actual depth of the amphibious unmanned platform is greater than the expected depth, in order to avoid the amphibious unmanned carrier platform from colliding with the seabed due to sudden changes in the seabed topography, if the difference between the sum of the actual depth and the actual height minus the expected depth is less than 0.5 meters, the amphibious unmanned carrier platform will change from depth-fixed navigation to 0.5-meter fixed-altitude navigation to ensure its safety during operation and avoid collision accidents caused by sudden changes in the seabed topography.

[0114] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0115] Specific implementation plan three: During high-speed navigation, the amphibious unmanned platform controls the heading of the amphibious unmanned platform by controlling the vertical rudder. If the vertical rudder is used for heading control, the amphibious unmanned platform needs to reach a certain speed to produce a suitable rudder effect. The heading control method of the amphibious unmanned platform at high speed is as follows:

[0116] In the rudder mode, the amphibious unmanned platform lacks the effect of horizontal thrust and is an under-actuated system. In order to control the heading of the vehicle, incremental PID control is used. The error between the actual heading angle and the expected value is used as the input of the PID controller, and the output is the corresponding rudder angle deflection. The expression is as follows:

[0117] Δθ(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2))

[0118] Among them, e(k) is the heading angle error at the current moment, e(k-1) is the heading angle error at the previous moment, e(k-2) is the heading angle error at the moment before that, and K p is the proportional gain term, K i is the integral gain term, K d is the differential gain term.

[0119] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0120] Specific implementation plan 4: Combination Figure 6 As shown in the figure, the in-situ steering of the amphibious unmanned platform is achieved through the main thrust differential, and the incremental PID control is also used. The error between the actual heading angle and the expected value is used as the input, and the output is the size of the resultant force that needs to be distributed between the two main thrust differentials, and then a reasonable torque is generated through thrust distribution:

[0121] In the main propulsion differential mode, the amphibious unmanned platform is also an underactuated system. In order to control the heading of the vehicle, incremental PID control is used. The error between the actual heading angle and the expected value is used as the input of the PID controller, and the output is the resultant force. The expression is as follows:

[0122] ΔF(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2))

[0123] Among them, e(k) is the heading angle error at the current moment, e(k-1) is the heading angle error at the previous moment, and e(k-2) is the heading angle error at the moment before that;

[0124] ΔF=|F 1 |+|F 2 |, where F 1 is the thrust distribution of the left main thruster 501, F 2 is the thrust distribution of the right main thruster 502, and F 1 +F 2 =0.

[0125] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0126] Specific implementation plan five: Combination Figure 7 As shown in the figure, the speed control of the multi-terrain unmanned platform mainly tests the speed control ability of the multi-terrain unmanned platform, including speed maintenance and maneuvering speed change ability. The actual speed and expected speed error of the multi-terrain unmanned platform are taken as input, and the output is the size of the resultant force that needs to be distributed between the two main thrusters.

[0127] In the multi-purpose unmanned platform speed control mode, the multi-purpose unmanned platform is also an under-actuated system. In order to control the speed of the vehicle, incremental PID control is used, and the error between the actual speed and the expected value is used as the input of the PID controller. The output is the resultant force, and its expression is as follows:

[0128] ΔF(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2))

[0129] Where e(k) is the speed error at the current moment, e(k-1) is the speed error at the previous moment, and e(k-2) is the speed error at the moment before that; ΔF = |F 1 |+|F 2 |, F 1 is the thrust distribution of the left main thruster 501, F 2 It is the thrust size allocated to the right main thruster 502.

[0130] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0131] Specific implementation plan six: Combination Figure 8As shown, the depth control of the amphibious unmanned platform is completed through the vertical thruster, horizontal rudder and main thruster. When the amphibious unmanned platform is sailing at a high speed, the horizontal rudder produces a steering effect, thereby controlling the amphibious unmanned platform to dive. When the amphibious unmanned platform needs to dive underground to a fixed depth, the vertical thruster is required to complete the depth control.

[0132] In the horizontal rudder mode, if the amphibious unmanned platform wants to float or dive, the horizontal rudder is used to control the longitudinal inclination angle of the amphibious unmanned platform, and the depth is controlled in coordination with the propulsion of the main thruster. The incremental PID control algorithm is used, and the depth error is used as the input of the PID controller to output the horizontal rudder angle.

[0133] Combined with the line of sight navigation method, the 10m ahead of the multi-habitat unmanned platform’s own X direction position is taken as the expected X d Direction position, the desired depth position is Z d , from which the displacement error equation can be obtained:

[0134] x e =xx d ,z e =zz d

[0135] According to the line of sight method, the desired pitch angle can be obtained, and its expression is as follows:

[0136] θ d =tan -1 ((zz d ) / (xx d )).

[0137] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0138] Specific implementation plan seven: The crawling of the amphibious unmanned platform to the target needs to be completed by the tracks. During the underwater crawling process, the steering of the amphibious unmanned platform is achieved by controlling the differential speed of the tracks through PID control, and the speed of the tracks is controlled by controlling the duty cycle of the track motor to achieve acceleration, deceleration, forward and backward maneuvers; in the underwater environment, the thrusters and tracks are combined to control the amphibious unmanned platform to crawl and maneuver close to the bottom of the water, and the pressure of the amphibious unmanned platform and the bottom of the water is increased by controlling the vertical thrusters to apply downward thrust, and the main thrust is controlled to assist the tracks in crawling in the water.

[0139] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0140] The upper platform of the multi-functional unmanned platform includes a closed electronic cabin 4 and an external frame 5 outside the electronic cabin 4. The electronic cabin 4 is arranged at the center of the external frame 5. A main control system, a navigation and positioning system, a communication system and a power control system are arranged in the electronic cabin 4. A water propulsion system is arranged on the external frame 5. A land power device is arranged below the upper platform.

[0141] The underwater propulsion system includes a left main propeller 501 and a right main propeller 502 symmetrically arranged on both sides of the central axis of the stern of the amphibious unmanned platform, and also includes a stern vertical propeller 503, a stern horizontal propeller 504, a bow vertical propeller 505 and a bow horizontal propeller 506. The propellers of the left main propeller 501 and the right main propeller 502 are horizontally facing the rear of the amphibious unmanned platform; the stern vertical propeller 503 is arranged at the bottom of the external frame 5 and the propeller of the stern vertical propeller 503 is facing the tracked chassis 2 of the land power device; the stern horizontal propeller 504 and The bow horizontal thrusters 506 are parallel to each other and are respectively located on both sides of the bow and stern of the amphibious unmanned platform; the propeller of the bow vertical thruster 505 is oriented perpendicular to the direction of the port side; the propeller of the bow horizontal thruster 506 is oriented perpendicular to the direction of the starboard side; the coordinated use of the six thrusters can realize the maneuvers of floating, diving and turning the bow of the amphibious unmanned platform; the thrusters are powered by the electronic cabin 4 for power and control signal power; the thrusters are watertight brushless DC motors powered by 48V, which can switch forward and reverse at any time, adopt closed-loop control, feedback thrust data in real time, and have an overcurrent protection mechanism;

[0142] The land power system includes a crawler chassis 2 and two crawler assemblies arranged below the crawler chassis 2, each crawler assembly includes a driving wheel 201, a guide wheel 203, a load wheel 202, a crawler 204 and a crawler motor, a plurality of load wheels 202 are evenly arranged between the driving wheel 201 and the guide wheel 203, and the driving wheel 201 and the guide wheel 203 are both located obliquely above the load wheels 202 at both ends, the crawler 204 is wrapped around the driving wheel 201, the guide wheel 203 and the outer side of the plurality of load wheels 202, the output end of the crawler motor is connected to the driving wheel 201, and the driving wheel 201, the guide wheel 203 and the load wheel 202 are all rotatably connected to the crawler chassis 2; the amphibious unmanned platform realizes forward and backward movement and left and right turning on land through the forward and backward rotation and differential speed of the two crawler assemblies;

[0143] The main control system includes a motion computer, a mission computer and a guidance computer. The motion computer is used to sample and process sensor data and feed back sensor data to the host computer; the mission computer is used to plan tasks, execute motion algorithms, record information of the multi-amphibious unmanned platform, and perform network communication and fault monitoring; the guidance computer is used to give guidance instructions, including the desired heading, desired speed and desired depth; the motion control algorithm of the multi-amphibious unmanned platform is run by the main control system, and the instructions generated after the calculation can be transmitted to the underwater propulsion system or the land power system, thereby controlling the movement of the multi-amphibious unmanned platform in water, on land or underwater;

[0144] The navigation and positioning system includes a GPS module in the electronic cabin 4, an inertial combined sensor module (including an inertial navigation sensor and a Doppler sensor 508 (DVL) located outside the cabin) and a depth meter; it is used to locate and calculate the attitude of the multi-habitat unmanned platform, and realize combined navigation through the fusion of multiple sensors;

[0145] The communication system includes a WiFi communication module and a wireless communication module, which are used to connect to the host computer through the communication system. The communication mode can be adjusted according to the environment. The hybrid communication mode can ensure reliable communication of the multi-habitat unmanned carrier platform in a large range;

[0146] The power control system includes a main single-chip control board and a relay matrix board. Each pin of the main single-chip control board controls a relay switch and then controls the power supply of each module. The relay board is equipped with a voltage divider module, which divides the 48V battery voltage into three lines: 48V main power electricity, 24V auxiliary power electricity and 5-12V control electricity, and controls them in stages through relays. The rated capacity of the battery is 300Wh, and the total capacity of the battery is 600Wh. The 48V main power electricity is responsible for supplying power to six thrusters and the 24V auxiliary power electricity is responsible for supplying power to high-power sensors and computers, and the 5-12V control electricity is responsible for supplying power to low-power sensors and single-chip microcomputers. The relay and the current are sampled and processed by the single-chip board and sent to the mission computer. The electronic cabin 4 is powered by dual power supplies to avoid the current reduction caused by the operation of multiple motors and affecting the overall performance of the multi-habitat unmanned platform. The power supply is a rechargeable high-energy battery, which is arranged in a battery cabin 3 arranged at the stern of the external frame 5. The power control system is responsible for power supply management of all power supply equipment and monitoring the battery voltage and capacity.

[0147] Preferably, a plurality of buoyancy blocks 6 are provided above the track chassis 2. The buoyancy blocks 6 are foam-type buoyancy materials. The amphibious unmanned platform can be balanced by the buoyancy blocks 6. The center of gravity and the center of buoyancy of the amphibious unmanned platform after balancing are located at the geometric center position. The overall balancing is slightly negative buoyancy, which is conducive to the amphibious unmanned platform to travel on the bottom.

[0148] Preferably, the track motor can be powered by a battery on the track chassis 2. The track chassis 2 is provided with a watertight control cabin. The control cabin has a track microcontroller that directly controls the track motor and communicates with the main control system through a wired network connection. The chassis control cabin is provided with a water cooling circulation system to cool the control cabin. The track motor is also equipped with an encoder to determine the movement speed of the track 204 by measuring the motor speed within a certain period of time.

[0149] Preferably, the electronic cabin 4 is a cylindrical stainless steel shell structure, which can effectively conduct heat in an underwater environment, and a heat dissipation module is provided in the electronic cabin 4; end covers with watertight piercing parts are provided at both ends of the shell of the electronic cabin 4; there are rubber watertight interfaces on the end covers, and electrical and signal connections can be established with external thrusters, sensors, and chassis components through watertight cables; the electronic cabin 4 uses sealing rings and vacuum treatment to ensure the internal sealing.

[0150] Preferably, for the controlled system, a PID controller based on radial basis function network tuning is set, the radial basis function network is synonymous with RBF radial basis function network, RBF network and RBF neural network, and the PID adaptive control based on RBF radial basis function network is an effective control method, which combines the simplicity of the PID controller and the nonlinear approximation ability of the RBF network, and can better adapt to the control requirements of various complex systems;

[0151] Since traditional PID control requires constant adjustment of parameters, it is difficult to achieve better results if the parameters are unreasonable. It takes a long time to achieve the best results. Therefore, on the basis of single neuron adaptive PID control, a PID controller based on RBF neural network tuning is used; compared with the single neuron adaptive PID controller, its advantage is that it is nonlinear; the control effect of PID controller depends on the three control effects of proportion, integration and differentiation, forming a relationship of mutual cooperation and mutual restriction in the control quantity, but the effect is not necessarily linear, so using nonlinear RBF can achieve better results; the arbitrary nonlinear expression ability of RBF neural network can realize PID control with the best combination by learning the system performance;

[0152] The radial basis function neural network is a three-layer feedforward network with a single hidden layer. The input to output is nonlinear, while the mapping from the hidden layer space to the output is linear, which speeds up the learning speed and avoids the local minimum problem. Its structure is as follows: Figure 4 As shown, in the RBF network structure, X = [x 1 ,x 2 ,...,x n ] T is the network input vector, and the radial basis vector of the hidden layer node of the RBF neural network is H = [h 1 ,h2 ...,h m ] T , where h j is the Gaussian basis function:

[0153]

[0154] The center vector of the jth node in the hidden layer of the RBF neural network is C j =[c j1 ,c j2 ,...,c jm ] T , let the base width vector of the hidden layer node of the RBF neural network be B = [b 1 ,b 2 ,....,b m ] T , where b j is the basis width parameter of hidden layer node j, and is a number greater than 0; the weight vector of the network is w = [w 1 ,w 2 ,...,w n ] T , the identified network output is:

[0155] y mout =w 1 h 1 +w 2 h 2 +...+w m h m

[0156] Combination Figure 5 The block diagram of the PID controller based on RBF network tuning is shown in the figure. The parameter adjustment algorithm in the network is NNI parameter adjustment. Suppose the theoretical output of the identified system at time k is y out (k), the output of network identification is y mout (k), then the identification system performance index function is:

[0157]

[0158] The iterative algorithm for obtaining the output weight, node center and node base width parameters by the gradient descent method is as follows:

[0159] The output weight expression is as follows:

[0160] w j (k) = w j (k-1)+η(y out (k)-y mout (k))h j +α(w j (k-1)-wj (k-2))+β(w j (k-2)-w j (k-3))

[0161] Among them, η is the learning rate; α and β are momentum factors, and their value ranges are both (0, 1);

[0162] Node base width change Δb j for:

[0163]

[0164] Node base width parameter b j for:

[0165] b j (k) = b j (k-1)+ηΔb j +α(b j (k-1)-b j (k-2))+β(b j (k-2)-b j (k-3))

[0166] Node center change Δc ji for:

[0167]

[0168] Node center c j for:

[0169] c ji =c ji (k-1)+ηΔc ji +α(c ji (k-1)-c ji (k-2))+β(c ji (k-2)-c ji (k-3))

[0170] Where cj=[c11,c12,c13…c1m] is the center vector value of the jth hidden layer neuron;

[0171] The PID parameter adjustment algorithm is as follows:

[0172] The control error is:

[0173] error(k)=u(k)-yout(k)

[0174] Among them, u(k) is the input of the controlled system, yout(k) is the output of the controlled system;

[0175] The 3 PID inputs are:

[0176] xc(1)=error(k)-error(k-1)

[0177] xc(2)=error(k)

[0178] xc(3)=error(k)-2error(k-1)+error(k-2)

[0179] The neural network tuning index is:

[0180]

[0181] By gradient descent method, we get three parameters K p ,K I ,K D The adjustment amount is:

[0182]

[0183]

[0184]

[0185] in, K is the Jacobian information of the controlled object, that is, the sensitivity of the object's output to the change of the control input, which can be identified by the RBF neural network; p is the proportional gain term, K i is the integral gain term, K d is the differential gain term;

[0186]

[0187] Taking the expected depth and the actual depth of the amphibious unmanned platform as the error, the RBF network structure is 3-6-1, and the three parameters of network identification are the control input increment, the depth at the current moment, and the depth information at the previous moment. The initial proportion, integration, and differentiation parameters are set to: 0.01, 0.02, and 0.03. When the simulation ends, it becomes 436.5727, 60.9628, and 1117. It can be seen that the three parameters will be continuously adjusted during the simulation process.

[0188] Specific implementation scheme eight: The present invention provides a two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control system, which has a program module corresponding to the above steps and executes the steps in the above-mentioned two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control method during operation.

[0189] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0190] Specific implementation scheme nine: The present invention provides a computer-readable storage medium, wherein the computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of a two-stage hybrid propulsion amphibious unmanned platform self-disturbance control method when called by a processor.

[0191] The other combinations and connection relationships of this embodiment are the same as those of the first embodiment.

[0192] Although the present invention is disclosed as above, the protection scope of the present invention is not limited thereto. Those skilled in the art may make various changes and modifications without departing from the spirit and scope of the present invention, and these changes and modifications will fall within the protection scope of the present invention.

Claims

1. A two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control method, characterized in that: The following steps are involved: S100, establish a dynamic model of an amphibious unmanned platform in water, The hydraulic power and the control force of the actuator are expressed as follows: Among them, M A is the additional mass matrix, C A (V) is the hydrodynamic Coriolis matrix, D(V) is the hydrodynamic damping matrix, g(Θ) is the gravity buoyancy matrix, and τ is the control force matrix of the actuator; in, The hydrodynamic damping is expressed as follows: D(V)V=D l V+uD u V+D nl (V)V Among them, D l is the linear damping matrix, D u is the damping matrix that increases with increasing longitudinal velocity, D nl (V) is the nonlinear damping matrix; in, Assume that the mass of the amphibious unmanned platform immersed in water is W = mg, where g is the acceleration due to gravity and the buoyancy is ρ is the water density, is the volume of the amphibious unmanned platform immersed in water; assuming that gravity and buoyancy are equal, that is, W = B, the buoyancy center of the amphibious unmanned platform is r B =[x B ,y B ,z B ] T , and y B =0, then we have the following vector: The amphibious unmanned platform is an underactuated amphibious unmanned platform, and the longitudinal thrust, bow moment and pitch moment are expressed in vector form as follows: τ=[τ u 000t q t r ] T Among them, τ u is the longitudinal thrust, τ q is the pitch moment, τ r is the bow turning moment; By simplifying the six-degree-of-freedom mathematical model, the horizontal plane mathematical model of the amphibious unmanned platform can be obtained, which can be expressed in the form of a differential equation group as follows: Where A = -d 22 v+(d 26 -uc 26 )r,B=(d 62 -uc 62 )vd 66 r+τ r ,and, S200, establishing an auto-disturbance rejection controller for a multi-habitat unmanned platform, the auto-disturbance rejection controller comprising a tracking differentiator, a linear extended state observer and a linear error feedback module, the tracking differentiator is used to arrange the transition process to smooth the sudden change part of the input signal; the extended state observer tracks the influence of the unknown part of the model and the external unknown disturbance by establishing an extended state quantity; the control quantity is given through the linear error feedback module to compensate for the disturbance, so that the multi-habitat unmanned platform can adjust its heading in real time, and output the current actual heading to re-introduce it into the linear extended state observer to continue the cyclic control.

2. According to the two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control method of claim 1, it is characterized by: In step S200, it includes: S210, the tracking differentiator receives the set desired heading angle information ψ d , calculating an approximate value x1 of the desired heading angle and a differential signal x2 of the approximate heading angle according to the kinematic model of step S100; S220, the linear extended state observer receives the actual heading angle information ψ and the input control variable u of the multi-amphibious unmanned platform, calculates the estimated values ​​of the actual heading of the multi-amphibious unmanned platform and its first-order differential, which are expressed as z1 and z2 respectively, and calculates the total disturbance z3 suffered by the multi-amphibious unmanned platform; S230, calculating the deviation e1 between x1 and the observed variable z1 obtained by the tracking differentiator and the deviation e2 between the corresponding differential signals x2 and z2, and obtaining e1=x1-z1, e2=x2-z2; S240, the linear error feedback module obtains the control component according to the error e1 and the error e2, and uses the estimated value z3 of the unknown disturbance to perform disturbance compensation on the control component to obtain the control amount u of the multi-habitat unmanned platform; S250, the amphibious unmanned platform adjusts its heading in real time after receiving the control value u, and outputs the current actual heading to the linear expansion state observer to continue the cyclic control.

3. The method for anti-disturbance control of an amphibious unmanned platform with two-stage hybrid propulsion according to claim 2 is characterized by: In step S210, a sliding window filtering algorithm is used to filter the data sampled by the sensor in the navigation and positioning system, including: Set a sampling window with a length of M and a filtering window with a length of N. After the sensor starts sampling, if the data, i.e., the number of sample points, is less than M, then the data in the sampling window is accumulated and the average value is calculated as the result after filtering; if the number of data sample points is greater than M, then the sampling data before time M is discarded, and the data in the sampling window is bubble sorted, and the maximum and minimum values ​​are removed. The remaining M-2 data form a filtering window, satisfying M-2=N. The data in the filtering window is accumulated and the average value is calculated to obtain the filtered result.

4. The method for anti-disturbance control of an amphibious unmanned platform with two-stage hybrid propulsion according to claim 3 is characterized by: When the amphibious unmanned platform is conducting depth-fixed navigation, if the difference between the sum of the actual depth and the actual height minus the expected depth is less than 0.5 meters, the amphibious unmanned carrier platform will change from depth-fixed navigation to 0.5-meter altitude-fixed navigation.

5. The method for anti-disturbance control of an amphibious unmanned platform with two-stage hybrid propulsion according to claim 4 is characterized by: The amphibious unmanned platform controls the bow direction of the amphibious unmanned platform by controlling the vertical rudder during high-speed navigation, including: In the rudder mode, incremental PID control is used. The error between the actual heading angle and the expected value is used as the input of the PID controller, and the output is the corresponding rudder angle deflection. The expression is as follows: Δθ(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2)) Among them, e(k) is the heading angle error at the current moment, e(k-1) is the heading angle error at the previous moment, e(k-2) is the heading angle error at the moment before that, and K p is the proportional gain term, K i is the integral gain term, K d is the differential gain term.

6. The method for anti-disturbance control of an amphibious unmanned platform with two-stage hybrid propulsion according to claim 5 is characterized by: The in-situ steering control of the amphibious unmanned platform is achieved through the main propulsion differential, using incremental PID control. The error between the actual heading angle and the expected value is used as the input of the PID controller, and the output is the resultant force, which is expressed as follows: ΔF(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2)) Among them, e(k) is the heading angle error at the current moment, e(k-1) is the heading angle error at the previous moment, and e(k-2) is the heading angle error at the moment before that; ΔF=|F1|+|F2|, wherein F1 is the thrust allocated to the left main thruster (501), F2 is the thrust allocated to the right main thruster (502), and F1+F2=0.

7. The method for anti-disturbance control of an amphibious unmanned platform with two-stage hybrid propulsion according to claim 6 is characterized by: The speed control of the multi-purpose unmanned platform includes speed maintenance and maneuvering speed change. Incremental PID control is used. The error between the actual speed and the expected value is used as the input of the PID controller, and the output is the resultant force. The expression is as follows: ΔF(k)=K p (err(k)-err(k-1))+K i err(k)+K d (err(k)-2err(k-1)+err(k-2)) Among them, e(k) is the speed error at the current moment, e(k-1) is the speed error at the previous moment, and e(k-2) is the speed error at the previous moment; ΔF=|F1|+|F2|, F1 is the thrust size allocated to the left main thruster (501), and F2 is the thrust size allocated to the right main thruster (502).

8. The method for anti-disturbance control of an amphibious unmanned platform with two-stage hybrid propulsion according to claim 7 is characterized by: In the horizontal rudder mode, the depth control of the amphibious unmanned platform is achieved through the vertical thruster, horizontal rudder and main thruster. The incremental PID control algorithm is used, with the depth error as the input of the PID controller and the horizontal rudder angle as the output. Combined with the line of sight navigation method, the 10m ahead of the multi-habitat unmanned platform’s own X direction position is taken as the expected X d Direction position, the desired depth position is Z d , and the displacement error equation is obtained: x e =x-x d ,z e =z-z d The desired trim angle is: θ d <tan -1 ((zz d ) / (xx d ))。 9. A two-stage hybrid propulsion amphibious unmanned platform anti-disturbance control system, characterized by: The system has a program module corresponding to the steps of any one of claims 1 to 8 above, and executes the steps in the above-mentioned two-stage hybrid propulsion amphibious unmanned platform self-disturbance rejection control method when running.

10. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of the self-disturbance rejection control method of the two-stage hybrid propulsion amphibious unmanned platform according to any one of claims 1 to 8 when called by a processor.

Images