Ocean robot course control method based on improved extended state observer, program, equipment and storage medium

Abstract

The invention belongs to the technical field of ocean robot motion control, and particularly relates to an ocean robot course control method based on an improved extended state observer, a program, equipment and a storage medium. According to the method, the internal structure of the extended state observer is improved by introducing the resonance observation item, so that the time-varying disturbance observation error is reduced; meanwhile, a self-adaptive law is set for observation disturbance frequency, adjustment is carried out according to observation errors, the observation errors of time-varying disturbance are reduced, and therefore the purpose of stabilizing course control is achieved. Compared with a controller based on a traditional extended state observer, the controller provided by the invention can well solve the problem that time-varying disturbance cannot be well observed due to limited bandwidth of the observer, so that the anti-interference performance of course control is enhanced.

Description

Technical Field

[0001] The present invention belongs to the technical field of marine robot motion control, and in particular relates to a marine robot heading control method, program, device and storage medium based on an improved extended state observer. Background Art

[0002] Unmanned Marine Vehicle (UMV) is a widely used marine vehicle. With the development of the current economic and technological fields related to the ocean, the demand for the development of marine robots with excellent performance has further increased. Marine robots have a wide range of operations and can complete operations in a variety of environments such as nearshore and offshore. The good implementation of motion control for marine robots is a prerequisite for ensuring the good implementation of various tasks, especially the heading control of under-actuated marine robots. At present, the most widely used heading control for marine robots is the PD controller. However, when marine robots are sailing in the ocean, they will be subject to many disturbances such as waves and currents. It is difficult to achieve a good anti-interference effect by relying solely on the PD controller.

[0003] Active Disturbance Rejection Control (ADRC) is a typical control technology based on extended state observer. Traditional ADRC consists of three parts: tracking differentiator, extended state observer (ESO), and non-linear state error feedback (NLSEF). Extended state observer plays a core role in ADRC. It can observe the state information of each order of the system and the disturbance it receives through the input and output information of the system, without the need to use the precise mathematical model of the system. The observed disturbance information includes both the internal disturbance of the system and the external disturbance it receives. According to the estimated disturbance information, by designing the corresponding disturbance compensation in the controller, the system containing unknown disturbance and uncertainty can be converted into a linear integrator series standard system. A large number of nonlinear functions are used in the extended state observer, and its design parameters are numerous, and the parameter setting is difficult, which is not convenient for engineering application. Subsequently, the linear extended state observer (LESO) structure was proposed, which has the excellent characteristics of few parameters and easy adjustment. Studies have shown that the active disturbance rejection controller based on the linear extended state observer also has excellent control effect.

[0004] However, for controllers based on traditional extended observers, only constants and slowly varying disturbances can be effectively estimated. When observing periodic time-varying disturbances, there is a time-delay observation error due to the limited bandwidth. This means that controllers based on traditional extended state observers can suppress the non-periodic disturbance part, but cannot effectively suppress the time-varying periodic disturbance part. The wave disturbances that marine robots are subject to during navigation can be regarded as the superposition of various periodic waves and some non-periodic disturbances after Fourier transform. Therefore, the observation time-delay error of periodic waves will lead to the inability to compensate for the disturbance well, resulting in oscillation in heading control.

[0005] Patent CN117031967A adopts an iterative learning ADRC method, which introduces an iterative learning extended state observer into the ADRC, replaces the original extended state observer in the ADRC with the iterative learning extended state observer, and combines the original tracking differentiator and state error feedback in the ADRC to perform control calculations; however, this method has a slow iterative learning process and takes a long time to control the heading of the marine robot, and does not consider the problem of disturbance observation lag. Therefore, compared with the above patent, this method is more suitable for the motion control of marine robots subject to wave disturbances.

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

[0007] The purpose of the present invention is to provide a method, program, device and storage medium for heading control of a marine robot based on an improved extended state observer in order to solve the problem that a traditional controller based on an extended state observer has a stable time lag error when observing variable disturbances due to bandwidth limitations.

[0008] A method for controlling a marine robot heading based on an improved extended state observer comprises 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 amount;

[0010] Step 2: Calculate the bow turning moment of the marine robot according to the control quantity output by the PD controller and the total disturbance observed by the extended state observer, substitute the bow turning moment into the dynamic equation and kinematic equation of the marine robot itself, and 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 true value and observed value of the heading angle of the marine robot at the current moment;

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

[0013] Step 5: Update the disturbance frequency and observed total disturbance of the extended state observer according to 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 observation value and the heading angle observation value at the next moment, and returns to step 1 to perform heading control at the next moment.

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

[0016]

[0017] Among them, ψ d (t) is the expected heading angle of the marine robot at the current moment t; is the heading angle observation value at time t output by the extended state observer;

[0018] The observation error e in step 3 q (t) is:

[0019]

[0020] Among them, ψ(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, the heading error e(t) in step 1 is input into the PD controller to perform the following calculation:

[0022]

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

[0024] Furthermore, the bow turning moment u(t) in step 2 is:

[0025]

[0026] in, is the total disturbance observed by 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 cut-off frequency; β 04 =αω0,α∈[0.2,0.6]; is the disturbance frequency of the extended state observer at time t.

[0031] Furthermore, the step 5 is specifically as follows:

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

[0033]

[0034] Updating the observation period perturbations of the extended state observer and observe non-periodic disturbances Then update the observed total disturbance

[0035]

[0036] in,

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

[0038]

[0039]

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

[0041] A computer device / equipment / system comprises 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-mentioned method for controlling the heading of a marine robot based on an improved extended state observer.

[0042] A computer-readable storage medium stores a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned method for controlling the heading of a marine robot based on an improved extended state observer.

[0043] A computer program product includes a computer program / instruction, which, when executed by a processor, implements the steps of the above-mentioned method for controlling the heading of a marine robot based on an improved extended state observer.

[0044] The beneficial effects of the present invention are:

[0045] The present invention improves the internal structure of the extended state observer by introducing a resonant observation term, thereby reducing the observation error of time-varying disturbances; at the same time, an adaptive law is set for the observation disturbance frequency, and it is adjusted according to the observation error to reduce the observation error of time-varying disturbances, thereby achieving the purpose of stable heading control. Compared with the controller based on the traditional extended state observer, the present invention can well solve the problem that the time-varying disturbance cannot be well observed due to the limited bandwidth of the observer itself, thereby enhancing the anti-interference ability of heading control. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 It is the overall algorithm block diagram of the present invention.

[0047] Figure 2 This is a diagram of the heading control process of the improved extended state observer in the present invention. DETAILED DESCRIPTION

[0048] The present invention is further described below in conjunction with the accompanying drawings.

[0049] Aiming at the problem that the traditional extended state observer has observation errors for time-varying disturbances such as waves and currents, the present invention proposes a heading control method for a marine robot based on an improved extended state observer. Figure 1 The improved control algorithm block diagram is presented. For the underactuated marine robot affected by wave disturbances, there is a time-delay stability error when using the traditional anti-disturbance control controller. The extended state observer is optimized using the adaptive resonant controller, so that uncertain periodic and non-periodic disturbances can be estimated. Therefore, the improved anti-disturbance control can suppress uncertain periodic and non-periodic disturbances at the same time, and does not rely on specific disturbance information and models, thereby achieving the attenuation of the disturbance of wave flow suffered by the marine robot during the external navigation process, and achieving the effect of stable control.

[0050] The present invention improves the internal structure of the extended state observer, introduces a resonant observation term for observation compensation, and thus reduces the error of the time-varying disturbance observation; an adaptive law is set for the observation disturbance frequency, so that the observation disturbance frequency value in the observer can be adjusted autonomously according to the observation error. The improved extended state observer control algorithm changes the internal structure of the extended state observer, sets an update law, and thus forms a new observation process:

[0051]

[0052] The adaptive law for observing the disturbance frequency is:

[0053]

[0054] The method for controlling the heading of a marine robot based on an improved extended state observer comprises 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 extended state observer to observe the non-periodic disturbance Disturbance frequency Observed total disturbance Angular velocity observations And the heading angle observation

[0056] Step 2: Obtain the expected heading angle ψ of the marine robot at the current time t d (t), according to 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 variable u0(t);

[0057]

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

[0059] Step 3: Based on the control quantity u0(t) and the observed total disturbance of the extended state observer Calculate the bow torque u(t) of the marine robot, and obtain the heading angle ψ(t) of the marine robot according to the dynamic equation and kinematic equation of the marine robot itself;

[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] Among them, β 04 =αω0, α∈[0.2,0.6], ω0 represents the 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 observe non-periodic disturbances Then update the observed total disturbance

[0070]

[0071] in,

[0072] Step 8: Calculate the angular velocity observation value of the extended state observer for the next moment and the heading angle observation

[0073]

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

[0075] Step 9: If the marine robot does not complete the task, return to step 2.

[0076] The present invention provides a method, program, device and storage medium for controlling the heading of a marine robot based on an improved extended state observer, which is suitable for the situation where the marine robot is disturbed by waves, currents and the like during navigation, resulting in unstable heading control. Due to bandwidth limitations, the controller based on the traditional extended state observer has a time lag error when observing time-varying disturbances. Therefore, the present invention mainly solves the problem that the traditional extended state observer cannot observe disturbances well, thereby causing instability of the heading control system. The present invention can eliminate uncertain external environmental disturbances and internal model perturbations, and optimize the internal structure of the extended observer by introducing a resonant observation term and setting an adaptive law for the disturbance observation frequency, so that uncertain periodic time-varying and non-periodic disturbances can be observed and estimated. The improved extended state observer controller can suppress uncertain periodic and non-periodic disturbances at the same time, and is independent of specific disturbances and model information, thereby attenuating the disturbances caused by external waves and current disturbances suffered by the marine robot during navigation, and achieving the purpose of stable heading control.

[0077] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. A method for controlling the heading of a marine robot based on an improved extended state observer, characterized in that: The following steps are involved: 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 amount; Step 2: Calculate the bow turning moment of the marine robot according to the control quantity output by the PD controller and the total disturbance observed by the extended state observer, substitute the bow turning moment into the dynamic equation and kinematic equation of the marine robot itself, and 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 true value and observed value of the heading angle of the marine robot at the current moment; Step 4: Update the resonant controller variables of the extended state observer according to the observation error; Step 5: Update the disturbance frequency and observed total disturbance of the extended state observer according to the updated resonant controller variables; Step 6: If the marine robot fails to complete the task, the updated extended state observer outputs the angular velocity observation value and the heading angle observation value at the next moment, and returns to step 1 to perform heading control at the next moment.

2. The method for controlling the heading of 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: Among them, ψ d (t) is the expected heading angle of the marine robot at the current moment t; is the heading angle observation value at time t output by the extended state observer; The observation error e in step 3 q (t) is: Among them, ψ(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 method for controlling the heading of a marine robot based on an improved extended state observer according to claim 2, characterized in that: The heading error e(t) in step 1 is input into the PD controller to perform the following calculation: Among them, u0(t) is the control quantity output by the PD controller; K p >0 is the gain coefficient; K d >0 is the differential coefficient; Δt is the time interval.

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

5. The method for controlling the heading of a marine robot based on an improved extended state observer according to claim 4, characterized in that: 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: l q1 (t+Δt)=l q1 (t)+Δt·l q2 (t) Where Δt is the time interval; ω0 is the bandwidth; ω c is the cut-off frequency; β 04 =αω0,α∈[0.2,0.6]; is the disturbance frequency of the extended state observer at time t.

6. The method for controlling the heading of a marine robot based on an improved extended state observer according to claim 5, characterized in that: The step 5 is specifically as follows: Update the perturbation frequency of the extended state observer Updating the observation period perturbations of the extended state observer and observe non-periodic disturbances Then update the observed total disturbance in, 7. The method for controlling the heading of a marine robot based on an improved extended state observer according to claim 6, characterized in that: The extended state observer updated in step 6 outputs the angular velocity observation value at the next moment and the heading angle observation for: among them,b 01 =3ω0,β 02 =ω0 2 。 8. A computer device / equipment / 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 7.

9. A computer-readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instructions are executed by a processor, the steps of the method according to any one of claims 1 to 7 are implemented.

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

Images