High-disturbance-rejection navigational speed and course control method for unmanned surface vehicle considering flow velocity compensation

Abstract

The invention discloses an unmanned surface vehicle high anti-interference course control method considering flow velocity compensation, and belongs to the field of unmanned surface vehicle motion control. According to the method, the internal structure of the extended state observer is improved, so that the phenomenon that the navigational speed and course control is unstable due to the fact that a traditional extended state observer cannot accurately observe disturbance in an inland river is solved. In the control method, a speed improvement expansion state observer and a course improvement expansion state observer are respectively arranged according to different orders of a speed and course control system, and a controller adopts an S-plane control method, so that the unmanned surface vehicle can realize high interference resistance during course and speed control of the unmanned surface vehicle; the whole control loop does not depend on an accurate mathematical model of the unmanned ship. The method provided by the invention is suitable for high-disturbance-rejection navigational speed and course control of the unmanned surface vehicle in typical navigation states such as downstream, countercurrent and crossflow in an inland waterway.

Description

Technical Field

[0001] The present invention belongs to the field of motion control of unmanned surface vehicles, and particularly relates to a high-disturbance rejection course control method for unmanned surface vehicles considering flow velocity compensation. Background Art

[0002] As an intelligent surface equipment, an unmanned surface vehicle (USV) has high autonomy, rich carrying capacity, good economy, and is easy to mass-produce. By using an unmanned surface vehicle to monitor channel data, efficient and safe water transportation guidance and channel inspection can be realized, the construction of digital channels can be promoted, and the modernization level and intelligent level of water governance capabilities can be improved.

[0003] An Extended State Observer (ESO) can observe the state information of each order and the disturbances suffered through the input and output information of the system, and at the same time does not require an accurate mathematical model. The observed information contains both internal disturbances and external disturbances of the system. Therefore, according to the observed disturbance information, by designing corresponding disturbance compensation in the control system, a system containing unknown disturbances and uncertainties can be transformed into a cascade integrator standard system.

[0004] Due to bandwidth limitations, traditional extended state observers can only accurately observe simple constant or some slow-varying disturbances, and cannot accurately observe the coupling terms of time-varying disturbances such as wind, waves, and currents, which leads to the phenomenon of control errors. Therefore, if the bandwidth of the extended state observer can be changed, the tracking of time-varying disturbances can be improved, the observation error can be reduced, and a good control disturbance rejection effect can be achieved.

[0005] Patent CN202111034047.6 adopts a neural network-based improved extended state observer, which introduces a neural network in the extended state observer to replace the traditional observation disturbance calculation. However, due to the introduction of the neural network, each neural node in the neural network needs to have a suitable weight coefficient, which causes the problem of overly complex parameters that are difficult to set. At the same time, the training of the network requires a certain amount of time, which also leads to the phenomenon of instability in the control process of the unmanned surface vehicle in the initial stage. The control parameters involved in the improvement of this patent are not so many and do not require much time to adjust the observation error.

[0006] In Patent CN202111320194.X, different orders of speed and heading control are considered, so two different extended state observers need to be designed. However, this method essentially still uses the traditional extended state observer, which will cause the speed and heading of the unmanned boat to be difficult to maintain stable when it is affected by external wind and wave disturbances. This patent improves the traditional extended state observer itself, ensuring the accuracy of observing various types of disturbances and improving the disturbance rejection ability of the controller.

[0007] In Patent CN202411790752.2, a heading-improved extended state observer is designed to achieve the heading stable disturbance rejection control of the unmanned boat. However, this method does not consider the instability and disturbances brought to the heading and speed respectively when the unmanned boat performs speed-heading coupling control. At the same time, the improved heading extended state observer cannot be used for speed disturbance rejection control, and the overall disturbance rejection closed-loop of the unmanned boat control system cannot be achieved.

[0008] Therefore, aiming at the problem of control interference caused by typical external environmental disturbances such as wind, waves and currents to the speed and heading of the unmanned boat in the inland waterway, a speed-heading control method for the unmanned boat based on the improved heading-speed extended state observer is proposed. The specific method is to change the internal structure of the extended state observer, that is, to supplement the periodic disturbance observation term and the adaptive law of the periodic disturbance frequency. At the same time, according to the different orders of heading control and speed control, two calculation methods of heading-improved extended state observer and speed-improved extended state observer are designed, so as to accurately observe the heading disturbance and speed disturbance, improve the disturbance rejection ability of the controller, and then achieve the effect of high disturbance rejection. Summary of the Invention

[0009] In view of the need to achieve high disturbance rejection control effects for the heading and speed of the unmanned boat during navigation in the inland waterway, this invention designs a high disturbance rejection control algorithm for the heading and speed.

[0010] This invention provides a high disturbance rejection speed-heading control method for a surface unmanned boat considering flow velocity compensation, including the following steps:

[0011] Step 1: Obtain the observed heading, observed speed of the current unmanned boat, and the desired speed and desired heading of the current unmanned boat, calculate the speed error by taking the difference between the desired speed and the observed speed, and calculate the heading error by taking the difference between the desired heading and the observed heading; input the heading error and the speed error into the heading S-surface controller and the speed S-surface controller respectively, and the heading S-surface controller and the speed S-surface controller output the initial heading controller output and the initial speed controller output;

[0012] Step 2: Subtract the observed heading disturbance and the observed speed disturbance of the heading improvement extended state observer and the speed improvement extended state observer from the output of the initial heading controller and the output of the initial speed controller respectively to obtain the final heading control output and the speed control output; substitute the heading control output and the speed control output into the unmanned boat mathematical model to obtain the current execution heading and speed of the unmanned boat;

[0013] Step 3: Calculate the heading observation error and the speed observation error according to the current execution heading and speed of the unmanned boat and the current observed heading and speed of the unmanned boat;

[0014] Step 4: Input the heading observation error and the speed observation error into the heading improvement extended state observer and the speed improvement extended state observer to update the variables of the heading improvement extended state observer and the variables of the speed improvement extended state observer;

[0015] Step 5: Update the disturbance frequency and the total heading disturbance of the heading improvement extended state observer and the disturbance frequency and the total speed disturbance of the speed improvement extended state observer according to the updated variables of the heading improvement extended state observer and the variables of the speed improvement extended state observer;

[0016] Step 6: The updated heading improvement extended state observer and speed improvement extended state observer output the observed heading angle and the observed speed at the next moment; judge whether the observed heading angle and the observed speed are the same as the desired heading and the desired speed. If they are the same, the unmanned boat completes the current control task; if they are not the same, the unmanned boat has not completed the task. Transmit the observed heading angle and the observed speed at the next moment to the heading S-surface controller and the speed S-surface controller respectively, and return to Step 1 for the heading control at the next moment.

[0017] Further, the heading error e ψ (t) is:

[0018]

[0019] where is the current observed heading of the unmanned boat; ψ d (t) is the current desired heading of the unmanned boat;

[0020] The speed error e u (t) is:

[0021]

[0022] where is the current observed speed of the unmanned boat; u d (t) is the current desired speed of the unmanned boat;

[0023] The heading S-surface controller outputs the control quantity T r0(t) is:

[0024]

[0025] where k r1 > 0, k r2 > 0 are the design parameters of the course S - surface controller;

[0026] The control quantity F u0 (t) output by the speed S - surface controller is:

[0027]

[0028] where k u1 > 0, k u2 > 0 are the design parameters of the speed S - surface controller.

[0029] Furthermore, in step 2, the improved extended state observer control algorithm for the course changes the internal structure of the extended state observer, sets the update law, and forms a new observation process:

[0030]

[0031] where l ψ1 > 0, l ψ2 > 0, l ψ3 > 0 are the parameters of the improved extended state observer for the course.

[0032] Furthermore, in step 2, the improved extended state observer control algorithm for the speed changes the internal structure of the extended state observer, sets the update law, and forms a new observation process:

[0033]

[0034] where l u1 > 0, l u2 > 0 are the parameters of the improved extended state observer for the speed.

[0035] Furthermore, in step 2, the course control output T r (t) obtained after compensating for the disturbance is:

[0036]

[0037] where is the total course disturbance observed by the improved extended observer for the course; b0 > 0 is the gain of the course control system; the speed control output F u (t) obtained after compensating for the disturbance is:

[0038]

[0039] where is the total disturbance of the observed ship speed of the ship speed improved extended observer; b1 > 0 is the ship speed control system gain. Further, in step 3, the course observation error eψ q (t) is:

[0040]

[0041] The ship speed observation error eu q (t) is:

[0042]

[0043] Further, in step 4, the updated variable of the course improved extended observer is:

[0044]

[0045] Where, is the designed cut-off frequency of the course disturbance;

[0046] The updated variable of the ship speed improved extended observer is:

[0047]

[0048] Where, is the designed cut-off frequency of the ship speed disturbance.

[0049] Further, in step 5, the disturbance frequency of the updated course improved extended state observer is:

[0050]

[0051] Where, K p1 > 0 is the gain coefficient; l ψ4 > 0 is the design parameter of the course improved extended state observer.

[0052] The updated observed total course disturbance is:

[0053]

[0054] Where, is the observed course periodic disturbance of the course improved extended observer; is the observed course periodic disturbance of the course improved extended observer;

[0055] The disturbance frequency of the updated ship speed improved extended state observer is:

[0056]

[0057] Among them, K p2 > 0 is the gain coefficient; l ψ4 > 0 is the design parameter of the speed improvement extended state observer.

[0058] The updated observed total speed disturbance

[0059]

[0060] Among them, is the observed speed periodic disturbance of the speed improvement extended observer; is the observed speed periodic disturbance of the speed improvement extended observer.

[0061] Furthermore, in step 6, the observed angular velocity and the heading angle at the next moment are:

[0062]

[0063] The observed speed at the next moment is:

[0064]

[0065] The present invention also provides a computer device / system, including a memory, a processor, and a computer program stored on the memory. When the processor executes the computer program, the steps of the high disturbance rejection course control method for a surface unmanned boat considering flow velocity compensation described in any one of the above are implemented.

[0066] The present invention also provides a computer-readable storage medium, on which a computer program / instruction is stored. When the computer program / instruction is executed by a processor, the steps of the high disturbance rejection course control method for a surface unmanned boat considering flow velocity compensation described in any one of the above are implemented.

[0067] The present invention also provides a computer program product, including a computer program / instruction. When the computer program / instruction is executed by a processor, the steps of the high disturbance rejection course control method for a surface unmanned boat considering flow velocity compensation described in any one of the above are implemented.

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

[0069] The high anti-disturbance heading control method for surface unmanned boats considering flow velocity compensation provided by the present invention, for unmanned boats navigating in inland rivers, the greatest impact on their speed and heading control is the external environment such as wind and rivers. These disturbances are often the coupling terms of strong steady disturbances and time-varying disturbances. If a traditional extended state observer is used, the disturbances cannot be accurately observed, and high anti-disturbance of heading and speed control cannot be achieved. Therefore, this algorithm improves the traditional extended state observer, supplements the observation of periodic disturbance terms, improves the disturbance observation accuracy, and thus improves the anti-disturbance ability of the control algorithm. At the same time, according to the different orders of heading and speed control, a heading improved extended state observer and a speed improved extended state observer are designed, thus realizing the high anti-disturbance control of heading and speed. Description of the Drawings

[0070] Figure 1 It is the control system diagram of the heading improved extended state observer and the speed improved extended state observer of the present invention;

[0071] Figure 2 It is the flow chart of the high anti-disturbance speed and heading control method for surface unmanned boats considering flow velocity compensation of the present invention;

[0072] Figure 3 It is the comparison diagram of heading control simulation of the high anti-disturbance speed and heading control method for surface unmanned boats considering flow velocity compensation of the present invention;

[0073] Figure 4 It is the comparison diagram of speed control simulation of the high anti-disturbance speed and heading control method for surface unmanned boats considering flow velocity compensation of the present invention. Detailed Embodiment

[0074] The following further describes the present invention with reference to the drawings.

[0075] The present invention improves the structure of the traditional extended state observer to achieve accurate observation of unknown strong steady disturbances and time-varying disturbances, and respectively designs an improved heading extended state observer and an improved speed extended state observer according to the differences in the orders of heading and speed control as Figure 1 shown, to achieve the observation of disturbing forces in the corresponding degrees of freedom of heading and speed;

[0076] Heading Improved Extended State Observer:

[0077]

[0078] Heading Controller:

[0079]

[0080] Finally, the heading control output is obtained:

[0081]

[0082] Speed improvement extended state observer:

[0083]

[0084] Speed controller:

[0085]

[0086] Finally, the speed control output is obtained:

[0087]

[0088] A high disturbance rejection speed and heading control method for surface unmanned vessels considering flow velocity compensation, as Figure 1 shown, specifically includes the following steps:

[0089] Step 1: Input the current desired heading ψ d (t) and the current desired speed u d (t), and respectively subtract them from the current observed heading and the observed speed to obtain the heading error and the speed error which are respectively used as the inputs of the heading S-surface controller and the speed S-surface controller, so as to calculate the output T of the heading controller r0 (t) and the output F of the speed controller u0 (t);

[0090] The output control quantity T of the heading S-surface controller r0 (t) is:

[0091]

[0092] where k r1 > 0, k r2 > 0 are the design parameters of the heading S-surface controller;

[0093] The output control quantity F of the speed S-surface controller u0 (t) is:

[0094]

[0095] where k u1 > 0, k u2 > 0 are the design parameters of the speed S-surface controller.

[0096] Step 2: Subtract the heading disturbances observed by the heading improvement extended state observer and the speed improvement extended state observer at this time from the obtained output T of the heading controller r0 (t) and the output F of the speed controller u0 (t) respectively and the speed disturbance to obtain the final heading and speed control outputs T r (t), F u (t). By calculating through the unmanned boat mathematical model, the actuator response at this time can be obtained, thereby changing the heading ψ(t) and the speed u(t);

[0097] The heading control output T r (t) is:

[0098]

[0099] where b0 > 0 is the gain of the heading control system;

[0100] The speed control output F u (t) is:

[0101]

[0102] where b1 > 0 is the gain of the speed control system.

[0103] Step 3: Subtract the current heading ψ(t) and speed u(t) of the unmanned boat from the observed heading and the observed speed respectively to obtain the heading observation error eψ q (t) and the speed observation error e u q (t), which are used as the inputs of the heading improved extended state observer and the speed improved extended state observer respectively;

[0104] The heading observation error eψ q (t) is:

[0105]

[0106] The speed observation error e u q (t) is:

[0107]

[0108] Step 4: According to the current observed heading observation error the speed observer error and the disturbance frequency of the heading improved extended state observer the disturbance frequency of the speed improved extended state observer and the respectively set cut-off frequencies and update the variables of the heading improved extended state observer and the variables of the speed improved extended state observer Then update the periodic disturbance term in the heading disturbance and the periodic disturbance term in the course and speed disturbances

[0109] The variable ζ of the improved extended state observer for course ψq1 (t + Δt), ζ ψq2 (t + Δt) is:

[0110]

[0111] where is the designed cut-off frequency of the course disturbance, is the design parameter of the course observer; and the observed periodic course disturbance

[0112] The variable of the improved extended observer for speed is:

[0113]

[0114] where is the designed cut-off frequency of the speed disturbance, l u4 > 0 is the design parameter of the speed observer; and the observed periodic speed disturbance

[0115] Step Five: After updating the variables of the course and speed observers, the observed course disturbance frequency and the speed disturbance frequency are updated through different adaptive laws.

[0116] Design the following update law to calculate the observed periodic course disturbance frequency:

[0117]

[0118] where K p1 > 0 is the gain coefficient.

[0119] Design the following update law to calculate the observed periodic speed disturbance frequency:

[0120]

[0121] where K p2 > 0 is the gain coefficient.

[0122] Step Six: Multiply the course observation error eψ q (t) and the speed observation error e u q (t) by the corresponding observer coefficients to obtain the aperiodic disturbance term of the course observation and the aperiodic disturbance term of the speed observation Then, respectively, with the observed periodic course disturbance and the course speed period disturbance are superimposed to obtain the total updated course observation disturbance and the total speed observation disturbance

[0123] Step 7: The total course disturbance obtained by observation The updated improved extended observer of the course outputs the observed angular velocity at the next moment and the observed course angle The total speed disturbance obtained by observation The updated improved extended observer of the course outputs the observed speed at the next moment

[0124]

[0125] Step 8: Determine whether the obtained observed course angle and the observed speed are the same as the desired course and the desired speed. If they are the same, the current control task is completed; otherwise, they are respectively transmitted to the course S-surface controller and the speed S-surface controller to complete the control at the next moment.

[0126] Embodiment 1

[0127] A high disturbance rejection course control method for surface unmanned boats considering flow velocity compensation, setting the current disturbance as d r (t) = sin(0.005t) + 2cos(0.001t) + 0.5sin(0.002t) + 0.5, d u (t) = 0.4sin(0.002t) + 0.2. The simulation results of the desired course being 60° in the first 200s and 90° in the next 200s, and the desired speed being 1m / s in the first 200s and 2m / s in the next 200s are as Figure 3 and Figure 4 shown. The specific process of the entire course speed disturbance scheme is as follows:

[0128] Step 1: The current course error is and the speed error is The control quantity output by the course S-surface controller is The output quantity of the speed S-surface controller is

[0129] Step 2: After compensating for the disturbance, the course control output obtained is The speed control output is

[0130] Step 3: Obtain the course observation error at this time Calculate the variable in the improved extended state observer of the course

[0131]

[0132] Among them, is the cut-off frequency of the heading disturbance of the design, is the design parameter of the heading observer, and the observed periodic heading disturbance

[0133] The speed observer error is obtained The variables in the improved extended state observer of the speed are calculated

[0134]

[0135] Among them, is the cut-off frequency of the speed disturbance of the design, l u4 >0 is the design parameter of the speed observer, and the observed periodic speed disturbance

[0136] Step 4: After obtaining the variables of the improved extended state observer of the heading The following update law is designed to calculate the frequency of the observed periodic heading disturbance:

[0137]

[0138] Among them, K p1 >0 is the gain coefficient.

[0139] After obtaining the variables of the improved extended state observer of the speed The frequency of the observed periodic speed disturbance is calculated according to the following update law:

[0140]

[0141] Among them, K p2 >0 is the gain coefficient.

[0142] Step 5: The improved extended state observer of the heading calculates the non-periodic heading disturbance l ψ3 >0 is the parameter of the heading observer, and the current observed heading disturbance is The improved extended state observer of the speed also calculates the non-periodic speed disturbance l u2 >0 is the parameter of the heading observer, and the current observed speed disturbance is

[0143] Step 6: According to the heading disturbance calculated by the improved extended state observer of the heading and the observation error e_ψq , the observed angular velocity and heading angle are calculated:

[0144]

[0145] The speed disturbance calculated by improving the extended state observer according to the ship speed and the observation error e_u p , the observed ship speed is calculated:

[0146]

[0147] Step 7: Determine whether the heading and ship speed have reached the stable desired heading and desired ship speed. If so, the current unmanned boat control task is completed; otherwise, the observed disturbances, heading angle, and ship speed are transmitted to the controller to complete the control at the next moment.

[0148] In particular, in some preferred embodiments of the present invention, a computer device is further provided, including a memory, a processor, and a computer program stored on the memory. When the processor executes the computer program, the steps of the high anti-disturbance heading control method for a surface unmanned boat considering flow velocity compensation described in any of the above embodiments are implemented.

[0149] In some other preferred embodiments of the present invention, a computer-readable storage medium is further provided, on which a computer program / instructions are stored. When the computer program is executed by a processor, the steps of the high anti-disturbance heading control method for a surface unmanned boat considering flow velocity compensation described in any of the above embodiments are implemented.

[0150] In summary, the traditional extended state observer can only act on constant disturbances or slow-varying disturbances. When a surface unmanned boat sails in an inland waterway, the disturbances are mainly caused by wind and rivers, and its mathematical model is a coupling of a strong constant model and a time-varying model. Compared with the heading and speed control using the traditional extended state observer, the method of the present invention improves the structure of the traditional extended state observer to achieve accurate observation of unknown strong constant disturbances and time-varying disturbances. According to the differences in the control orders of heading and speed, a heading extended state observer and a speed extended state observer are respectively designed to realize the observation of disturbing forces in the corresponding degrees of freedom of heading and speed, and the application scenario is broadened. The application scenario is a surface unmanned boat affected by a strong flow field sailing in inland waters / harbors, etc.

[0151] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the processes of the embodiments of the high anti-disturbance heading control method for a surface unmanned boat considering flow velocity compensation as described above, which will not be repeated here.

[0152] Those of ordinary skill in the art can understand that all or part of the processes in the methods of the above embodiments can be completed by instructing relevant hardware through a computer program. The computer program can be stored in a non-volatile computer-readable storage medium. When the computer program is executed, it can include the method of using a bionic robotic fish to identify and track aquatic biological communities as described above.

[0153] In the description of this specification, the description with reference to terms such as "one embodiment", "some embodiments", "example", "specific example", or "some examples" means that the specific features, structures, materials, or characteristics described in connection with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic descriptions of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described can be combined in any one or N embodiments or examples in a suitable manner. In addition, without contradiction, those skilled in the art can combine and combine the different embodiments or examples described in this specification and the features of different embodiments or examples.

[0154] In addition, the terms "first" and "second" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance or implicitly indicating the quantity of the indicated technical features. Thus, the features defined with "first" and "second" can explicitly or implicitly include at least one of the features. In the description of the present invention, the meaning of "N" is at least two, such as two, three, etc., unless otherwise clearly and specifically defined.

[0155] Any process or method description shown in the flowchart or described in other ways herein can be understood as representing a module, segment, or part of code that includes one or more executable instructions for implementing a customized logic function or process. The scope of the preferred embodiments of the present invention includes additional implementations, where the functions can be executed in a substantially simultaneous manner or in the reverse order according to the functions involved, rather than in the order shown or discussed, which should be understood by those skilled in the art to which the embodiments of the present invention belong.

[0156] The logic and / or steps represented in the flowchart or otherwise described herein can be considered as a definitional sequence list of executable instructions for implementing logical functions, and can be specifically implemented in any computer-readable medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other systems that can fetch and execute instructions from the instruction execution system, apparatus, or device), or in combination with these instruction execution systems, apparatuses, or devices. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transmit a program for use by or in combination with an instruction execution system, apparatus, or device. More specific examples (non-exhaustive list) of computer-readable media include the following: an electrical connection portion (electronic device) having one or N wirings, a portable computer disk cartridge (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disc read-only memory (CDROM). Additionally, the computer-readable medium can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other media, followed by editing, interpretation, or otherwise processing as appropriate, and then storing it in a computer memory.

[0157] It should be understood that various parts of the present invention can be implemented by hardware, software, firmware, or a combination thereof. In the above embodiments, the N steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, any one or a combination of the following techniques well known in the art can be used: discrete logic circuits having logic gate circuits for implementing logical functions on data signals, application-specific integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field programmable gate arrays (FPGAs), etc.

[0158] Those of ordinary skill in the art of this technology can understand that all or part of the steps carried by the method of implementing the above embodiments can be completed by instructing relevant hardware through a program, and the program can be stored in a computer-readable storage medium. When the program is executed, it includes one or a combination of the steps of the method embodiment.

[0159] In addition, each functional unit in various embodiments of the present invention may be integrated into one processing module, or each unit may exist physically alone, or two or more units may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of a software functional module. When the integrated module is implemented in the form of a software functional module and sold or used as an independent product, it may also be stored in a computer-readable storage medium.

[0160] The above-mentioned storage medium may be a read-only memory, a magnetic disk, an optical disk, etc. Although the embodiments of the present invention have been shown and described above, it can be understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those of ordinary skill in the art can make changes, modifications, substitutions, and variations to the above embodiments within the scope of the present invention.

Claims

1. A high disturbance rejection speed and heading control method for unmanned surface vessels considering flow velocity compensation, characterized in that: It includes the following steps: Step 1: Obtain the observed heading, observed speed of the current unmanned boat, as well as the desired speed and desired heading of the current unmanned boat. Calculate the speed error by taking the difference between the desired speed and the observed speed, and calculate the heading error by taking the difference between the desired heading and the observed heading. Input the heading error and the speed error into the heading S-surface controller and the speed S-surface controller respectively. The heading S-surface controller and the speed S-surface controller output the initial heading controller output and the initial speed controller output; Step 2: Subtract the observed heading disturbance and the observed speed disturbance observed by the heading improved extended state observer and the speed improved extended state observer from the initial heading controller output and the initial speed controller output respectively to obtain the final heading control output and the speed control output. Substitute the heading control output and the speed control output into the unmanned boat mathematical model to obtain the current execution heading and speed of the unmanned boat; Step 3: Calculate the heading observation error and the speed observation error based on the current execution heading and speed of the unmanned boat and the current observed heading and speed of the unmanned boat; Step 4: Input the heading observation error and the speed observation error into the heading improved extended state observer and the speed improved extended state observer to update the variables of the heading improved extended state observer and the variables of the speed improved extended state observer; Step 5: Update the disturbance frequency of the heading improved extended state observer and the total heading disturbance, and the disturbance frequency of the speed improved extended state observer and the total speed disturbance according to the updated variables of the heading improved extended state observer and the variables of the speed improved extended state observer; Step 6: The updated heading improved extended state observer and the speed improved extended state observer output the observed heading angle and the observed speed at the next moment; Judge whether the observed heading angle and the observed speed are the same as the desired heading and the desired speed. If they are the same, the unmanned boat completes the current control task. If they are not the same, the unmanned boat has not completed the task. Transmit the observed heading angle and the observed speed at the next moment to the heading S-surface controller and the speed S-surface controller respectively, and return to Step 1 for the heading control at the next moment.

2. The high anti-disturbance speed and heading control method for a surface unmanned boat considering flow velocity compensation according to claim 1, characterized in that: The course error e ψ (t) in step 1 is as follows: Among them, is the current observation course of the unmanned boat; ψ d (t) is the expected course of the unmanned boat at present; Heading speed error e u (t) is as follows: Among them, is the observed speed of the current unmanned boat; u d (t) is the desired speed of the current unmanned boat; The heading S surface controller outputs a control quantity T r0 (t) as follows: where k r1 > 0, k r2 > 0 are the design parameters of the course S surface controller; The speed S surface controller outputs a control quantity F u0 (t) as follows: where k u1 > 0, k u2 > 0 are the design parameters of the speed S surface controller.

3. The high anti-disturbance speed and heading control method for a surface unmanned boat considering flow velocity compensation according to claim 1, characterized in that: In Step 2, the control algorithm of the heading improved extended state observer changes the internal structure of the extended state observer, sets an update law, and forms a new observation process: where, l ψ1 > 0, l ψ2 > 0, l ψ3 > 0 are the parameters of the course improvement and expansion state observer.

4. The high anti-disturbance speed and heading control method for the unmanned surface vehicle considering flow velocity compensation according to claim 3, characterized in that: In Step 2, the control algorithm of the speed improved extended state observer changes the internal structure of the extended state observer, sets an update law, and forms a new observation process: where l u1 > 0, l u2 > 0 are the parameters of the speed improvement expansion state observer.

5. The high anti-disturbance speed and heading control method for unmanned surface vehicle considering flow velocity compensation according to claim 4, characterized in that: In step 2, the heading control output T r (t) is as follows: wherein, is the total disturbance of the observed course of the course improvement and expansion observer; b0 > 0 is the gain of the course control system; The speed control output F u (t) after compensating for the disturbance is as follows: wherein, is the total disturbance of the observed ship speed of the ship speed improvement and expansion observer; b1>0 is the ship speed control system gain.

6. The high anti-disturbance speed and heading control method for a surface unmanned boat considering flow velocity compensation according to claim 5, characterized in that: In step 3, the heading observation error eψ q (t) is as follows: The observed ship speed error eu q (t) is as follows:

7. The high anti-disturbance speed and heading control method for a surface unmanned boat considering flow velocity compensation according to claim 6, characterized in that: In step 4, the variable for updating the heading-improved extended state observer is as follows: Among them, is the designed cut-off frequency of the heading disturbance; l ψ4 > 0 is the design parameter of the heading improvement extended state observer; The updated ship speed improvement and extended state observer variable is as follows: Among them, is the cut-off frequency of the designed speed disturbance; l u4 > 0 is the design parameter of the speed improvement and expansion state observer.

8. The high anti-disturbance speed and course control method for a surface unmanned boat considering flow velocity compensation according to claim 7, characterized in that: In step 5, update the disturbance frequency of the heading improvement and expansion state observer It is: Among them, K p1 > 0 is the gain coefficient; l ψ4 > 0 is the design parameter of the course improvement expansion state observer. The updated observed total heading disturbance is as follows: Among them, is the observed course periodic disturbance of the course improvement extended observer; is the observed course periodic disturbance of the course improvement extended observer; Update the disturbance frequency of the improved extended state observer for ship speed It is: Among them, K p2 > 0 is the gain coefficient; l ψ4 > 0 is the design parameter of the speed improvement expansion state observer. The updated observed total speed disturbance is as follows: Among them, is the observed speed periodic disturbance of the speed improvement extended observer; is the observed speed periodic disturbance of the speed improvement extended observer.

9. The high anti-disturbance speed and heading control method for a surface unmanned boat considering flow velocity compensation according to claim 8, characterized in that: In step 6, the observed angular velocity and the heading angle are as follows: The observed speed at the next moment is as follows:

10. A computer device / apparatus / system, comprising a memory, a processor, and a computer program stored on the memory, characterized in that: The processor executes the computer program to implement the steps of the method according to any one of claims 1 to 9.

Images