A method, program, device, and storage medium for heading control of a marine robot based on an improved extended state observer.

Abstract

This invention belongs to the field of marine robot motion control technology, specifically relating to a method, program, device, and storage medium for heading control of a marine robot based on an improved extended state observer. This invention improves the internal structure of the extended state observer by introducing a resonant observation term, reducing the observation error for time-varying disturbances. Simultaneously, it sets an adaptive law for the observed disturbance frequency and adjusts it according to the observation error, further reducing the observation error for time-varying disturbances, thereby achieving stable heading control. Compared to controllers based on traditional extended state observers, this invention effectively solves the problem of limited bandwidth of the observer itself, which prevents accurate observation of time-varying disturbances, thus enhancing the anti-interference capability of heading control.

Description

Technical Field

[0001] This invention belongs to the field of marine robot motion control technology, specifically relating to a method, program, device, and storage medium for marine robot heading control based on an improved extended state observer. Background Technology

[0002] Unmanned marine vehicles (UMVs) are widely used marine transport vehicles. With the development of marine-related economic and technological fields, the demand for high-performance marine robots is further increasing. Marine robots have a wide operating range and can perform tasks in various environments, including nearshore and open ocean. Effective motion control of marine robots is a prerequisite for the successful completion of various tasks, especially the heading control of underactuated marine robots. Currently, the most commonly used heading control method for marine robots is the PD controller. However, marine robots are subject to numerous disturbances such as waves and currents during ocean navigation, making it difficult for PD controllers alone to achieve good anti-interference effects.

[0003] Active Disturbance Rejection Control (ADRC) is a typical control technique based on extended state observers. A traditional ADRC consists of three parts: a tracking differentiator, an extended state observer (ESO), and a non-linear state error feedback (NLSEF). The extended state observer plays a central role in ADRC. It can observe the system's state information at various orders and the disturbances it experiences through the system's input and output information, without requiring a precise mathematical model of the system. The observed disturbance information includes both internal system disturbances and external disturbances. Based on the estimated disturbance information, by designing corresponding disturbance compensation in the controller, the system containing unknown disturbances and uncertainties can be transformed into a standard system with linear integrators in series. However, the extended state observer employs numerous nonlinear functions, resulting in a large number of design parameters and significant tuning difficulties, making it inconvenient for engineering applications. Subsequently, the Linear Extended State Observer (LESO) structure was proposed, which has the excellent characteristics of having few parameters and being easy to adjust. Furthermore, research has shown that the active disturbance rejection controller based on the Linear Extended State Observer also has excellent control performance.

[0004] However, controllers based on traditional extended state observers can only effectively estimate constants and slowly varying disturbances. When observing periodic time-varying disturbances, time-delay observation errors exist due to limited bandwidth. This means that controllers based on traditional extended state observers can suppress aperiodic disturbances but cannot effectively suppress time-varying periodic disturbances. Furthermore, the wave disturbances experienced by marine robots during navigation can be viewed as a superposition of various periodic waves and some aperiodic disturbances after Fourier transform. Therefore, the time-delay error in observing periodic waves leads to inadequate disturbance compensation, resulting in oscillations in heading control.

[0005] Patent CN117031967A employs an iterative learning active disturbance rejection control (ADRC) method. This method introduces an iterative learning extended state observer into the ADRC, replacing the original extended state observer in the ADRC, and combines it with the original tracking differentiator and state error feedback unit in the ADRC for control calculations. However, this method has a slow iterative learning process, requiring a long time for heading control of marine robots, and it does not consider the problem of disturbance observation lag. Therefore, compared with the aforementioned patent, this method is more suitable for motion control of marine robots subjected to wave disturbances.

[0006] Patent CN111564997B uses a resonant controller to control and suppress harmonic disturbances in the generator current. However, this method is suitable for the anti-disturbance control of motors against harmonic currents and cannot be directly used for the anti-disturbance control of marine robots subjected to wave disturbances. Therefore, compared with the above patent, this improved method can be applied to the motion control of marine robots subjected to wave disturbances. Summary of the Invention

[0007] The purpose of this invention is to address the problem of stability delay error in traditional controllers based on extended state observers when observing variable disturbances due to bandwidth limitations, and to provide a method, program, device and storage medium for the heading control of marine robots based on an improved extended state observer.

[0008] A heading control method for a marine robot based on an improved extended state observer includes the following steps:

[0009] Step 1: Obtain the expected heading angle of the marine robot at the current moment and the heading angle observation value output by the extended state observer. Subtract the two to calculate the heading error. Input the heading error into the PD controller, and the PD controller outputs the control quantity.

[0010] Step 2: Calculate the bow torque of the marine robot based on the control quantity output by the PD controller and the total disturbance observed by the extended state observer. Substitute the bow torque into the dynamic equation and kinematic equation of the marine robot itself to calculate the true value of the heading angle of the marine robot at the current moment.

[0011] Step 3: Calculate the observation error based on the actual and observed values ​​of the ocean robot's current heading angle;

[0012] Step 4: Update the resonant controller variables of the extended state observer based on the observation error;

[0013] Step 5: Update the perturbation frequency and total observed perturbation of the extended state observer based on the updated resonant controller variables;

[0014] Step 6: If the marine robot fails to complete the task, the updated extended state observer outputs the angular velocity and heading angle observation values ​​for the next moment, and returns to Step 1 to perform heading control for the next moment.

[0015] Furthermore, the heading error e(t) in step 1 is:

[0016]

[0017] Where, ψ d (t) represents the expected heading angle of the marine robot at time t. The heading angle observation value at time t is output by the extended state observer;

[0018] In step 3, the observation error e q (t) is:

[0019]

[0020] Wherein, ψ(t) is the true value of the heading angle at the current time t, obtained by solving the dynamic equation and kinematic equation of the marine robot itself in step 2.

[0021] Furthermore, in step 1, the heading error e(t) is input to the PD controller for the following calculation:

[0022]

[0023] Where u0(t) is the control quantity output by the PD controller; K p >0 represents the gain coefficient; K d >0 represents the differential coefficient; Δt represents the time interval.

[0024] Furthermore, the steering torque u(t) in step 2 is:

[0025]

[0026] in, b0 represents the total observed disturbance of the extended state observer; b0 is the controller gain.

[0027] Furthermore, in step 4, the resonant controller variable l of the extended state observer is updated according to the observation error. q1 (t+Δt) and l q2 (t+Δt) is:

[0028] l q1 (t+Δt)=l q1 (t)+Δt·l q2 (t)

[0029]

[0030] Where Δt is the time interval; ω0 is the bandwidth; ω c β is the cutoff frequency; 04 =αω0, α∈[0.2,0.6]; Let be the perturbation frequency of the extended state observer at time t.

[0031] Furthermore, step 5 specifically includes:

[0032] Update the perturbation frequency of the extended state observer

[0033]

[0034] Update the observation period perturbation of the extended state observer and observation of aperiodic disturbances This will update the total observed perturbation.

[0035]

[0036] in,

[0037] Furthermore, in step 6, the updated extended state observer outputs the angular velocity observation value for the next time step. and heading angle observations for:

[0038]

[0039]

[0040] Where, β 01 =3ω0,β 02 =ω0 2 .

[0041] A computer device / apparatus / system includes a memory, a processor, and a computer program stored in the memory, wherein the processor executes the computer program to implement the steps of the above-described method for heading control of a marine robot based on an improved extended state observer.

[0042] A computer-readable storage medium having a computer program / instructions stored thereon, which, when executed by a processor, implements the steps of the above-described method for heading control of a marine robot based on an improved extended state observer.

[0043] A computer program product includes a computer program / instructions that, when executed by a processor, implement the steps of the above-described method for heading control of a marine robot based on an improved extended state observer.

[0044] The beneficial effects of this invention are as follows:

[0045] This invention improves the internal structure of the extended state observer by introducing a resonant observation term, reducing the observation error for time-varying disturbances. Simultaneously, it sets an adaptive law for the observed disturbance frequency and adjusts it according to the observation error, further reducing the observation error for time-varying disturbances, thereby achieving stable heading control. Compared to controllers based on traditional extended state observers, this invention effectively solves the problem of limited bandwidth in the observer, which prevents accurate observation of time-varying disturbances, thus enhancing the anti-interference capability of heading control. Attached Figure Description

[0046] Figure 1 This is a flowchart of the overall algorithm of the present invention.

[0047] Figure 2 This is a diagram illustrating the heading control process of the improved extended state observer in this invention. Detailed Implementation

[0048] The present invention will now be further described with reference to the accompanying drawings.

[0049] To address the problem of observation errors in traditional extended state observers for time-varying disturbances such as waves and currents, this invention proposes a heading control method for marine robots based on an improved extended state observer. Figure 1 The improved control algorithm block diagram is presented. Addressing the time-delay stability error inherent in traditional active disturbance rejection controllers (ADRCs) for underactuated marine robots affected by wave disturbances, an adaptive resonant controller is used to optimize the extended state observer, thereby estimating uncertain periodic and aperiodic disturbances. Therefore, the improved ADRC can simultaneously suppress uncertain periodic and aperiodic disturbances without relying on specific disturbance information or models, thus attenuating the wave and current disturbances experienced by the marine robot during navigation and achieving control stability.

[0050] This invention improves the internal structure of the extended state observer by introducing a resonant observation term for observation compensation, thereby reducing the error in observing time-varying disturbances. An adaptive law is set for the observed disturbance frequency, allowing the observer to autonomously adjust the observed disturbance frequency value based on the observation error. The improved extended state observer control algorithm alters the internal structure of the extended state observer and sets an update law, thus forming a new observation process:

[0051]

[0052] The adaptive law for the observed perturbation frequency is:

[0053]

[0054] The heading control method for marine robots based on an improved extended state observer includes the following steps:

[0055] Step 1: Set the time interval Δt, bandwidth ω0, and cutoff frequency ω c Initialize t = t0, initialize the resonant controller variable l q1 (t0) and l q2 (t0), Initialize the observed aperiodic perturbation of the extended state observer Disturbance frequency Total observed disturbance angular velocity observations and heading angle observations

[0056] Step 2: Obtain the expected heading angle ψ of the marine robot at time t. d (t), based on the heading angle observation value output by the extended state observer. Calculate the heading error e(t). The heading error e(t) is input into the PD controller, and the PD controller outputs the control quantity u0(t);

[0057]

[0058] Among them, K p >0 represents the gain coefficient; K d >0 represents the differential coefficient;

[0059] Step 3: Based on the control quantity u0(t) and the total disturbance observed by the extended state observer Calculate the bow turning torque u(t) of the marine robot, and obtain the heading angle ψ(t) of the marine robot based on its own dynamic equations and kinematic equations;

[0060]

[0061] Where b0 is the controller gain;

[0062] Step 4: Based on the heading angle ψ(t) of the marine robot and the heading angle observation value output by the extended state observer... Calculate the observation error e q (t),

[0063] Step 5: Update the resonant controller variable l q1 (t+Δt) and l q2 (t+Δt);

[0064] l q1 (t+Δt)=l q1 (t)+Δt·l q2 (t)

[0065]

[0066] Where, β 04 =αω0, α∈[0.2,0.6], ω0 represents bandwidth;

[0067] Step 6: Update the perturbation frequency of the extended state observer

[0068]

[0069] Step 7: Update the observation period perturbation of the extended state observer and observation of aperiodic disturbances This will update the total observed perturbation.

[0070]

[0071] in,

[0072] Step 8: Calculate the angular velocity observations of the extended state observer for the next time step. and heading angle observations

[0073]

[0074] Where, β 01 =3ω0,β 02 =ω0 2

[0075] Step 9: If the ocean robot fails to complete the task, return to step 2.

[0076] This invention provides a method, program, device, and storage medium for course control of marine robots based on an improved extended state observer. It is applicable to situations where the course control of a marine robot is unstable due to disturbances such as waves and currents during navigation. Traditional extended state observer controllers suffer from time-delay errors when observing time-varying disturbances due to bandwidth limitations. Therefore, this invention primarily addresses the problem that traditional extended state observers cannot effectively observe disturbances, leading to instability in the course control system. This invention eliminates uncertain external environmental disturbances and internal model perturbations. By introducing a resonant observation term and setting an adaptive law for the disturbance observation frequency, the internal structure of the extended observer is optimized, enabling the observation and estimation of uncertain periodic and aperiodic disturbances. The improved extended state observer controller can simultaneously suppress uncertain periodic and aperiodic disturbances, without relying on specific disturbance and model information, thus attenuating the interference caused by external waves and currents during navigation and achieving stable course control.

[0077] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A heading control method for a marine robot based on an improved extended state observer, characterized in that, Includes the following steps: Step 1: Obtain the expected heading angle of the marine robot at the current moment and the heading angle observation value output by the extended state observer. Subtract the two to calculate the heading error. Input the heading error into the PD controller, and the PD controller outputs the control quantity. Step 2: Calculate the bow torque of the marine robot based on the control quantity output by the PD controller and the total disturbance observed by the extended state observer. Substitute the bow torque into the dynamic equation and kinematic equation of the marine robot itself to calculate the true value of the heading angle of the marine robot at the current moment. Step 3: Calculate the observation error based on the actual and observed values ​​of the ocean robot's current heading angle; Step 4: Update the resonant controller variables of the extended state observer based on the observation error; The resonant controller variable l q1 (t+Δt) and l q2 (t+Δt) is: l q1 (t+Δt)=l q1 (t)+Δt·l q2 (t) Where Δt is the time interval; ω0 is the bandwidth; ω c β is the cutoff frequency; 04 =αω0, α∈[0.2,0.6]; Let e ​​be the perturbation frequency of the extended state observer at time t. q (t) represents the observation error; Step 5: Update the perturbation frequency and total observed perturbation of the extended state observer based on the updated resonant controller variables; The perturbation frequency of the updated extended state observer Among them, K p >0 represents the gain coefficient; Update the observation period perturbation of the extended state observer and observation of aperiodic disturbances This will update the total observed perturbation. in, Step 6: If the marine robot fails to complete the task, the updated extended state observer outputs the angular velocity and heading angle observation values ​​for the next moment, and returns to step 1 to perform heading control for the next moment; The updated extended state observer outputs the angular velocity observation value at the next moment. and heading angle observations for: Where, β 01 =3ω0,β 02 =ω0 2 b0 is the controller gain, and u0(t) is the control quantity output by the PD controller.

2. The heading control method for a marine robot based on an improved extended state observer according to claim 1, characterized in that: The heading error e(t) in step 1 is: Where, ψ d (t) represents the expected heading angle of the marine robot at time t. The heading angle observation value at time t is output by the extended state observer; In step 3, the observation error e q (t) is: Wherein, ψ(t) is the true value of the heading angle at the current time t, obtained by solving the dynamic equation and kinematic equation of the marine robot itself in step 2.

3. The heading control method for a marine robot based on an improved extended state observer according to claim 2, characterized in that: In step 1, the heading error e(t) is input to the PD controller for the following calculation: Where u0(t) is the control quantity output by the PD controller; K p >0 represents the gain coefficient; K d >0 represents the differential coefficient; Δt represents the time interval.

4. The heading control method for a marine robot based on an improved extended state observer according to claim 2, characterized in that: The turning torque u(t) in step 2 is: in, b0 represents the total observed disturbance of the extended state observer; b0 is the controller gain.

5. A computer system comprising a memory, a processor, and a computer program stored in 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 4.

6. A computer-readable storage medium having a computer program / instructions stored thereon, characterized in that: When the computer program / instructions are executed by the processor, they implement the steps of the method described in any one of claims 1 to 4.

7. A computer program product comprising a computer program / instructions, characterized in that: When the computer program / instructions are executed by the processor, they implement the steps of the method described in any one of claims 1 to 4.

Images