Harmonic-wave-oriented active-disturbance-rejection motion control method for underwater mechanical arm

Abstract

The invention relates to the technical field of motion planning and control of underwater mechanical arms, in particular to a harmonic-wave-oriented active-disturbance-rejection motion control method of an underwater mechanical arm. Comprising the following steps: establishing an underwater mechanical arm kinematics model; constructing a harmonic noise mathematical expression and a harmonic signal mathematical expression according to the harmonic noise frequency measured by the measuring tool, and designing a noise compensator capable of adaptively generating harmonic signals; according to the noise compensator capable of adaptively generating the harmonic signals, combining an underwater mechanical arm kinematics model, and adding error feedback into a kinematics scheme at the same time, designing an underwater mechanical arm active-disturbance-rejection motion control method for harmonic waves; and the lower computer controller drives each joint and tracks a target trajectory according to a calculation result of the harmonic-wave-oriented underwater mechanical arm active-disturbance-rejection motion control method, and controls the underwater mechanical arm to complete a given tail end operation task. According to the method, harmonic noise can be effectively handled, and the anti-interference capability and the control precision of the underwater mechanical arm are improved.

Description

Technical Field

[0001] This invention relates to the field of motion planning and control technology for underwater robotic arms, and in particular to a method for self-disturbance rejection motion control of underwater robotic arms oriented towards harmonics. Background Technology

[0002] As a key component of underwater operations, underwater robotic arms are widely used in marine development, scientific research, and engineering operations, playing a crucial role in tasks such as seabed mineral extraction and underwater infrastructure maintenance. Their motion control technology is one of the core components of an underwater robotic arm system. As underwater activities expand into complex and unknown environments, harmonic noise generated by environmental disturbances such as waves and currents increasingly interferes with control systems, reducing control accuracy, causing motion deviations in the robotic arm, and even preventing it from completing its intended tasks.

[0003] Given that harmonic noise is quite typical in underwater environments, how to effectively identify and compensate for harmonic noise interference has become an urgent problem to be solved in the research on motion control of underwater robotic arms. Summary of the Invention

[0004] In view of this, the purpose of this invention is to propose an underwater robotic arm self-disturbance rejection motion control method for harmonics, so as to solve the problem that the existing technology is difficult to effectively deal with underwater harmonic noise, resulting in low control accuracy and trajectory deviation of the robotic arm.

[0005] Based on the above objectives, the technical solution adopted by the present invention is: a method for self-disturbance rejection motion control of an underwater robotic arm oriented towards harmonics, comprising the following steps:

[0006] S1. Establish the kinematic model of the underwater robotic arm;

[0007] S2. Based on the harmonic noise frequency measured by the measuring tool, construct the mathematical expression for harmonic noise and the mathematical expression for harmonic signal, and design a noise compensator that can adaptively generate harmonic signals.

[0008] S3. Based on the noise compensator that can adaptively generate harmonic signals, combined with the kinematic model of the underwater manipulator, and with error feedback added to the kinematic scheme, design an underwater manipulator self-disturbance motion control method oriented towards harmonics.

[0009] S4. The lower-level controller drives each joint and tracks the target trajectory based on the calculation results of the underwater robotic arm self-disturbance rejection motion control method for harmonics, and controls the underwater robotic arm to complete the given end-effector operation task.

[0010] Preferably, the specific operation of "establishing the kinematic model of the underwater robotic arm" is as follows: First, taking an underwater four-axis robotic arm as an example, the kinematic equation of the underwater robotic arm is: , This indicates the time of the end effector of the underwater quadcopter. The expected trajectory Indicates the time of the underwater quadcopter. The joint angles, that is, including four joint angles , , , , The kinematic mapping function is represented by the equation; secondly, the kinematic equations of the velocity layer are obtained by taking the time derivative of the above equation. , This indicates the time of the end effector of the underwater quadcopter. Expected speed, The Jacobian matrix represents the structure of an underwater quadcopter. Indicates the time of the underwater quadcopter. The joint velocity, which includes the angular velocities of the four joints. , , , .

[0011] Preferably, the mathematical expression for the harmonic noise is: , among which, time, This indicates harmonic noise containing multiple frequencies, indicated by the subscript. , Indicates the number of harmonic noises; Indicates the first One harmonic noise; Indicates the first The amplitude of each harmonic noise; Indicates the first The frequency of harmonic noise; Indicates the first The phase of a harmonic noise.

[0012] Preferably, the mathematical expression for the harmonic signal is: ,in, This indicates a harmonic signal containing multiple frequencies. Represented as a periodic signal, subscript , Indicates the number of harmonic signals. Indicates the first One harmonic signal.

[0013] Preferably, the formula for the harmonic noise compensator that can adaptively generate harmonic signals is:

[0014]

[0015] in, express Time derivative, Indicates the use of compensation for the first harmonic signal Interference signals, express The time derivative is defined as , express The time derivative.

[0016] Preferably, the formula for the harmonic-oriented underwater robotic arm's self-disturbance rejection motion control method is as follows:

[0017]

[0018] in, , Represents the Jacobian matrix of an underwater four-axis robotic arm The pseudo-inverse matrix; express The transpose of the matrix; The position error of the robotic arm's end effector is defined as follows: , This represents a nonlinear mapping function derived from the structural parameters of the robotic arm; and This represents the feedback gain for the position error.

[0019] The beneficial effects of this invention are as follows: Compared with the prior art, this invention achieves accurate compensation of harmonic signals by designing a noise compensator that can adaptively estimate amplitude and phase using only harmonic frequencies, thereby specifically suppressing harmonic interference and effectively improving the system's anti-interference capability; by combining kinematic models and error feedback to construct an active anti-interference control scheme, it ensures that the underwater robotic arm can still perform end-effector tasks with high precision in complex underwater environments; it significantly enhances the environmental adaptability and operational reliability of the robotic arm, and is especially suitable for underwater operation scenarios with high requirements for operational accuracy and system stability. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only for this invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0021] Figure 1 This is a schematic diagram of the process of the present invention. Detailed Implementation

[0022] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the invention will be further described in detail below with reference to specific embodiments. It should be noted that, unless otherwise defined, the technical or scientific terms used in this invention should have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains.

[0023] like Figure 1 As shown, a method for active disturbance rejection motion control of an underwater robotic arm oriented towards harmonics includes:

[0024] S1. Establish the kinematic model of the underwater robotic arm.

[0025] Taking an underwater quadcopter as an example, the kinematic equations of the underwater robotic arm are:

[0026]

[0027] This indicates the time of the end effector of the underwater quadcopter. The expected trajectory Indicates the time of the underwater quadcopter. The joint angles, that is, including four joint angles , , , , Let represent the kinematic mapping function. Taking the time derivative of the above equation yields the kinematic equations for the velocity layer:

[0028]

[0029] This indicates the time of the end effector of the underwater quadcopter. Expected speed, The Jacobian matrix represents the structure of an underwater quadcopter. Indicates the time of the underwater quadcopter. The joint velocity, which includes the angular velocities of the four joints. , , , .

[0030] S2. Based on the harmonic noise frequencies measured by the measuring tools, construct mathematical expressions for harmonic noise and harmonic signals, and design a noise compensator that can adaptively generate harmonic signals.

[0031] In practical applications, the amplitude and phase of harmonic noise signals are difficult to measure, but the frequency is relatively easy to obtain using measuring tools. Assuming the amplitude and phase of the harmonic noise are unknown, but its frequency is known, the corresponding mathematical expression for the harmonic noise is:

[0032]

[0033] Among them, time, This indicates harmonic noise containing multiple frequencies, indicated by the subscript. , Indicates the number of harmonic noises; Indicates the first One harmonic noise; Indicates the first The amplitude of each harmonic noise; Indicates the first The frequency of harmonic noise; Indicates the first The phase of a harmonic noise.

[0034] To effectively optimize the interference generated by harmonic noise and improve the system's stability and anti-interference capability, a harmonic signal needs to be constructed to cancel out the noise or suppress interference in the system. The mathematical expression for the harmonic signal constructed based on the harmonic noise is:

[0035]

[0036] in, This indicates a harmonic signal containing multiple frequencies. Represented as a periodic signal, subscript , Indicates the number of harmonic signals. Indicates the first One harmonic signal.

[0037] Based on the harmonic noise formula and harmonic signal formula constructed from unknown amplitude and phase information, the design of a harmonic noise compensator that can adaptively generate harmonic signals is as follows:

[0038]

[0039] in, express Time derivative, Indicates the use of compensation for the first harmonic signal Interference signals, express The time derivative is defined as , express The time derivative.

[0040] S3. Based on the aforementioned noise compensator capable of adaptively generating harmonic signals, and combined with the kinematic model of the underwater robotic arm, while incorporating error feedback into the kinematic scheme, a harmonic-oriented underwater robotic arm active disturbance rejection motion control method is designed.

[0041] Based on a noise compensator capable of adaptively generating harmonic signals, and combined with the kinematic model of an underwater four-axis robotic arm, while incorporating error feedback into the kinematic scheme, a harmonic-oriented active disturbance rejection motion control method for an underwater robotic arm is designed as follows:

[0042]

[0043] in, , Represents the Jacobian matrix of an underwater four-axis robotic arm The pseudo-inverse matrix; express The transpose of the matrix; The position error of the robotic arm's end effector is defined as follows: , This represents a nonlinear mapping function derived from the structural parameters of the robotic arm; and This represents the feedback gain for the position error.

[0044] In the proposed harmonic-oriented underwater manipulator active disturbance rejection motion control method, the first dynamic equation It can solve the joint velocities of an underwater quadcopter in real time; other dynamic equations are used to estimate harmonic signals in real time, and these signals cancel each other out with the constructed harmonic signals. This further counteracts the impact of harmonic noise on the system, thereby ensuring that the underwater quadcopter will not make mistakes due to harmonic noise interference during the execution of end-effector tasks.

[0045] S4. Based on the calculation results of the harmonic-oriented underwater robotic arm's active disturbance rejection motion control method, the lower-level controller drives each joint and tracks the target trajectory, controlling the underwater robotic arm to complete the given end-effector task.

[0046] The lower-level controller is equipped with a set of driving algorithms and multiple joint driving modules. It calculates the joint velocities of the underwater quadrilateral manipulator at different times using a harmonic-oriented underwater manipulator active disturbance rejection motion control method. The driving algorithm converts this velocity data into driving commands for the underwater quadrilateral manipulator. Even in the presence of harmonic noise interference, it controls each joint and tracks the target trajectory, enabling the underwater quadrilateral manipulator to complete the given end-effector operation task.

[0047] Those skilled in the art should understand that the discussion of any of the above embodiments is merely exemplary and is not intended to imply that the scope of the invention (including the claims) is limited to these examples; within the framework of the invention, the technical features of the above embodiments or different embodiments can also be combined, the steps can be implemented in any order, and there are many other variations of the different aspects of the invention as described above, which are not provided in the details for the sake of brevity.

[0048] This invention is intended to cover all such substitutions, modifications, and variations that fall within the broad scope of the appended claims. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for self-disturbance rejection motion control of an underwater robotic arm oriented towards harmonics, characterized in that, Includes the following steps: S1. Establish the kinematic model of the underwater robotic arm; S2. Based on the harmonic noise frequency measured by the measuring tool, construct the mathematical expression for harmonic noise and the mathematical expression for harmonic signal, and design a noise compensator that can adaptively generate harmonic signals. S3. Based on the noise compensator that can adaptively generate harmonic signals, combined with the kinematic model of the underwater robotic arm, and with error feedback added to the kinematic scheme, a harmonic-oriented underwater robotic arm self-disturbance motion control method is designed. S4. The lower-level controller drives each joint and tracks the target trajectory based on the calculation results of the underwater robotic arm self-disturbance rejection motion control method for harmonics, and controls the underwater robotic arm to complete the given end-effector operation task.

2. The method according to claim 1, characterized in that, The specific steps for "establishing the kinematic model of the underwater robotic arm" are as follows: First, taking an underwater four-axis robotic arm as an example, the kinematic equations of the underwater robotic arm are: , This indicates the time of the end effector of the underwater quadcopter. The expected trajectory Indicates the time of the underwater quadcopter. The joint angles, that is, including four joint angles , , , , The first expression represents the kinematic mapping function; secondly, by differentiating the above equation over time, the kinematic equations for the velocity layer are obtained. , This indicates the time of the end effector of the underwater quadcopter. Expected speed, The Jacobian matrix represents the structure of an underwater quadcopter. Indicates the time of the underwater quadcopter. The joint velocity, which includes the angular velocities of the four joints. , , , .

3. The method according to claim 1, characterized in that, The mathematical expression for the harmonic noise is: , among which, time, This indicates harmonic noise containing multiple frequencies, indicated by the subscript. , Indicates the number of harmonic noises; Indicates the first One harmonic noise; Indicates the first The amplitude of each harmonic noise; Indicates the first The frequency of harmonic noise; Indicates the first The phase of a harmonic noise.

4. The method according to claim 1, characterized in that, The mathematical expression for the harmonic signal is: ,in, This indicates a harmonic signal containing multiple frequencies. Represented as a periodic signal, subscript , Indicates the number of harmonic signals. Indicates the first One harmonic signal.

5. The method according to claim 1, characterized in that, The formula for the harmonic noise compensator that can adaptively generate harmonic signals is: in, express Time derivative, Indicates the use of compensation for the first harmonic signal Interference signals, express The time derivative is defined as , express The time derivative.

6. The method according to claim 1, characterized in that, The formula for the underwater robotic arm's active disturbance rejection motion control method oriented towards harmonics is as follows: in, , Represents the Jacobian matrix of an underwater four-axis robotic arm The pseudo-inverse matrix; express The transpose of the matrix; The position error of the robotic arm's end effector is defined as follows: , This represents a nonlinear mapping function derived from the structural parameters of the robotic arm; and This represents the feedback gain for the position error.

Images