A control method for underwater swimming manipulator based on load estimation

Abstract

The present invention relates to the field of manipulator control technology, and in particular to a method for controlling an underwater mobile manipulator based on load estimation, comprising: establishing a kinematic model and a dynamic model of the manipulator based on the motion data of the manipulator; determining whether the degree of environmental interference meets requirements based on underwater noise, and determining an adjustment method if it does not meet the requirements; obtaining the current position and posture; calculating the output parameters of an integral sliding mode controller and the compensation output parameters of an extended state observer; calculating the torque of the manipulator; calculating the thrust values ​​of several propellers of the manipulator; calculating the load mass of the manipulator; updating the position and posture of the manipulator based on the thrust values ​​and the load mass of the manipulator, and obtaining the updated position and posture. The present invention improves the operational stability and control accuracy of the manipulator.

Description

Technical Field

[0001] The present invention relates to the technical field of robotic arm control, and in particular to a method for controlling an underwater swimming robotic arm based on load estimation. Background Art

[0002] In the existing technology, underwater swimming manipulators mainly rely on thrusters to adjust their own posture when performing operational tasks. However, the dead zone characteristics of the thrusters may weaken their motion performance. The thruster dead zone refers to the situation where the thrusters cannot generate effective thrust within a certain input range, which greatly increases the difficulty of motion control of underwater swimming manipulators. Therefore, studying the thruster dead zone compensation problem of underwater swimming manipulators is the key to improving their operational performance in complex marine environments. After the underwater swimming manipulator grabs the load, its dynamic characteristics such as the manipulator mass distribution and inertia parameters will change, which may interfere with the motion balance of the underwater swimming manipulator. For the anti-disturbance control problem of underwater robots after grabbing the load, existing design solutions include but are not limited to proportional integral derivative control, model predictive control and sliding mode control.

[0003] Chinese Patent Publication No.: CN117140507A discloses an energy-optimal path planning algorithm for an underwater swimming manipulator in a complex environment, comprising the following steps: Step S1, establishing a forward kinematics model of the underwater swimming manipulator, wherein the model selects a north-east coordinate system to establish a reference coordinate system for the underwater swimming manipulator's posture; Step S2, performing a first-level path planning on the base of the underwater swimming manipulator, wherein the first-level path planning obtains random sampling points by combining a Gaussian and an artificial potential field on the basis of a Q-RRT* algorithm to reduce the probability of generating redundant nodes, and the first-level path planning is performed by I nformed-RRT* node rejection strategy is used to accelerate the convergence of the forward kinematics model of the underwater swimming manipulator; step S3, under the first-level path planning condition, the second-level path planning is performed on the base and each joint angle. The second-level path planning is based on the first-level path planning and iterates the model several times to reduce energy consumption to obtain the energy-optimal path planning for the base of the underwater swimming manipulator so that the manipulator can accurately avoid obstacles; wherein the base is the tail link of the underwater swimming manipulator, and the first-level path planning condition is to complete the first-level path planning and generate the first-level path. It can be seen that the energy-optimal path planning algorithm for the underwater swimming manipulator in the complex environment has the problem that it is difficult to maintain motion stability under complex environmental disturbances due to the characteristics of the underwater swimming manipulator such as the light base and multiple joints. Summary of the Invention

[0004] To this end, the present invention provides an underwater swimming manipulator control method based on load estimation to overcome the problem in the prior art that the underwater swimming manipulator has a light base, multiple joints and other characteristics, making it difficult to maintain motion stability under complex environmental disturbances.

[0005] To achieve the above objectives, the present invention provides a method for controlling an underwater swimming manipulator based on load estimation, comprising:

[0006] Step S1, establishing a kinematic model and a dynamic model of the robotic arm according to the motion data of the robotic arm;

[0007] Step S2, determining whether the degree of environmental interference meets the requirements based on the underwater noise, and determining an adjustment method if it does not meet the requirements, including adjusting the posture change speed of the manipulator or adjusting the data fusion range of the kinematic model and the dynamic model;

[0008] Step S3, obtaining the current position and posture of the robotic arm according to the adjustment method;

[0009] Step S4, calculating the output parameters of the integral sliding mode controller and the compensation output parameters of the extended state observer based on the kinematic model, the dynamic model and the current posture;

[0010] Step S5, calculating the torque of the manipulator according to the output parameters of the integral sliding mode controller and the compensation output coefficient of the extended state observer;

[0011] Step S6, performing thrust distribution on a plurality of thrusters of the manipulator according to the torque calculation to obtain a plurality of thrust values;

[0012] Step S7, calculating the load mass of the robotic arm according to the compensation output parameters of the extended state observer;

[0013] Step S8, adjusting the robotic arm to an updated posture according to the thrust value and the load mass of the robotic arm;

[0014] Step S9: If the updated posture is inconsistent with the target posture, repeat steps S3 to S8 until the robotic arm reaches the target posture.

[0015] Furthermore, the step S2 includes:

[0016] Obtaining underwater noise at a position of the extended state observer;

[0017] Comparing the underwater noise with a preset first underwater noise and a preset second underwater noise respectively;

[0018] If the underwater noise is greater than the preset first underwater noise, it is determined that the environmental interference level does not meet the requirement;

[0019] When the underwater noise is greater than the preset second underwater noise, reducing the posture change speed of the robotic arm;

[0020] When the underwater noise is greater than the preset first underwater noise and less than or equal to the preset second underwater noise, preliminarily determining that the accuracy of the expanded state observer does not meet the requirement, and obtaining a sensitivity change of the expanded state observer within a single detection cycle;

[0021] If the sensitivity change is greater than the preset change, it is determined that the accuracy of the extended state observer does not meet the requirements, and the data fusion range of the kinematic model and the dynamic model is increased.

[0022] Among them, the posture change speed of the robotic arm is negatively correlated with the underwater noise, the data fusion range is positively correlated with the sensitivity change; the sensitivity change of the expanded state observer within a single detection cycle is the difference between the sensitivity at the beginning of the detection cycle and the sensitivity at the end of the detection cycle.

[0023] Furthermore, the establishing of the kinematic model of the robotic arm includes:

[0024] Define the state vector of the manipulator in the world coordinate system {W}, define the velocity vector of the manipulator in the base coordinate system {B1}, and define the motion rotation of each link of the manipulator;

[0025] The position and posture of each link {Bi} of the manipulator in the world coordinate system {W} are calculated based on the state vector and the velocity vector. The position and posture are expressed as follows:

[0026]

[0027] Among them, H i is the position of connecting rod i, and is the rotation matrix and position vector of the i-th link coordinate system {Bi} relative to the world coordinate system {W}, 0 m×n Represents an m-by-n zero matrix.

[0028] Furthermore, the establishing of the kinetic model includes:

[0029] Establishing a generalized inertia matrix of the robotic arm;

[0030] Establishing an expression of the dynamic model according to the generalized inertia matrix;

[0031] Calculating the torque according to the expression of the dynamic model;

[0032] Wherein, the expression of the kinetic model is: The expression of the moment is τ=M(u AdaISMC +u ESO ),

[0033] M is the generalized inertia matrix, x1 and x2 are the state vector and velocity vector in the base coordinate system {B1}, τ is the torque, and N includes the uncertainty vector of the kinematic model and the external disturbance factor vector; u AdaISMC is the output parameter of the adaptive integral sliding mode controller, u ESO is the compensation output parameter of the extended state observer.

[0034] Furthermore, the calculating of the compensation output parameter of the extended state observer includes:

[0035] Define the extended state vector For z=[z1 z2 z3] T =[x1 x2 N] T , where z3 includes the extended state quantity of unknown external disturbance and model uncertainty;

[0036] The state observation value vector of the extended state observer is designed according to the extended state vector:

[0037]

[0038] in, is the state observation value vector; ρ1, ρ2, ρ3 are the gains of the extended state observer;

[0039] Determine the compensation output parameter of the extended state observer based on the state observation value vector

[0040] Furthermore, the calculating of the output parameters of the integral sliding mode controller includes:

[0041] The base link tracking error of the manipulator is defined as e = ξ - ξ d , where ξ d is the desired position and angle of each joint of the base link in the world coordinate system {W};

[0042] The integral sliding surface is determined according to the desired posture and the desired angles of each joint: Where c1 is the positive definite diagonal matrix of the proportional gain of the integrated sliding mode surface, and c2 is the positive definite diagonal matrix of the integral gain of the integrated sliding mode surface;

[0043] The derivative of the integral sliding surface is obtained: Right now

[0044] The output parameters of the integral sliding mode controller are determined based on the derived integral sliding mode surface:

[0045] Among them, K s is the diagonal gain matrix sgn(*) is a sign function, and its specific form is: Switching gain matrix adjusted for the update law

[0046] The update law is designed as follows:

[0047]

[0048] in, for The i-th value on the diagonal, is the rate of change of the adaptive gain, and χ i is a positive real number tending to zero; ε i is the boundary layer;

[0049] When|s i |<ε i When , it is determined that the robotic arm reaches the sliding surface.

[0050] Furthermore, the step S5 includes:

[0051] Step S51, defining control input parameters of the actuator of the robotic arm;

[0052]

[0053] Among them, u thr Indicates n t The control input parameters of the thrusters of the manipulator, u q Represents the control input parameters of n-1 joint motors;

[0054] Step S52, calculating the torque of the robotic arm according to the control input parameters of the actuator;

[0055] Wherein, the torque of the robotic arm is τ=Tu;

[0056] Where T is the control allocation matrix, specifically,

[0057]

[0058] Wherein, B is the thrust distribution matrix of the propeller, Β joint is the joint torque distribution matrix of the robotic arm;

[0059] Step S53: Determine the first six dimensions of the torque τ according to the control allocation matrix as τ thr =Bu thr , where u thr is the thrust value of the propeller;

[0060] Step S54, solving the first six dimensions of the torque τ to obtain a general solution for the thrust value of the propeller;

[0061] Wherein, the general solution is: thr =a1ξ1+a2ξ2+ξ0,

[0062] in, is a homogeneous linear system of equations The basic solution system, a1, a2 are the coefficients corresponding to ξ1, ξ2, ξ0 is the non-homogeneous linear equation system τ thr =Bu thr The special solution of .

[0063] Furthermore, the step S6 includes:

[0064] Step S61: Establishing the objective function of the thrust distribution model for the robotic arm:

[0065]

[0066] Among them, u thri =a1ξ 1i +a2ξ 2i +ξ 0i represents the thrust value of the i-th thruster;

[0067] Step S62: introducing auxiliary variable u into the objective function i (i=1,2,…,n), and add constraint u i ≥|u thri |;

[0068] Step S63: Determine the final form of the objective function according to the constraint conditions:

[0069] Step S64, set dead zone constraint u thri ≤d-oru thri ≥d + , where d + >0 is the forward dead zone of the thruster, d - <0 is the reverse dead zone of the propeller;

[0070] Step S65: When the thrust value is within the range of d-≤u thri ≤d + When , the thrust value actually output by the propeller is zero;

[0071] Step S66: introducing an integer auxiliary decision variable y for each thrust value i and z i ,

[0072] in, i=1,2,…,n, and y i +z i =1;

[0073] Step S67, according to the integer auxiliary decision variable y i and z i Identify linear constraints:

[0074]

[0075] Where Ω≥10 6 ;

[0076] Step S68, introduce the standard form of linear programming, expressed as:

[0077]

[0078] stx≤x c or x≥x c ,

[0079] Among them, J is the objective function of linear programming, x is the vector of decision variables, c is the coefficient vector of decision variables, x c is an inequality constraint;

[0080] Step S69, according to the integer auxiliary decision variable y i and z i , and the standard form of linear programming constructs the mixed integer linear programming form and is expressed as:

[0081]

[0082] Among them, x Z is x c Integer decision variables in ;

[0083] Step S610: construct the mixed integer linear programming form of the propeller according to the mixed integer linear programming form, and the expression is:

[0084]

[0085] Solve the mixed integer linear programming form of the thruster and obtain the optimal state quantity a 1_optimal and a 2_optimal ;

[0086] Step S611: 1_optimal and the a 2_optimal Substitute the general solution expression of the thrust value of the propeller and obtain the thrust value u of each propeller of the manipulator thr =a 1_optimal ξ1+a 2_optimal ξ2+ξ0.

[0087] Furthermore, the step S7 includes:

[0088] Step S71, establishing the load parameters of the robotic arm according to the dynamic model;

[0089]

[0090] Where m = m0 + m p is the total mass of the manipulator, m0 is the mass of the manipulator when it is not grabbing the load, and m p is the load mass, is the acceleration of each degree of freedom of the base link in the world coordinate system {W}, τ x , τ y , τ z is the first three dimensions of the moment τ, F x 、F y 、F z is the complex environmental disturbance, b0 is the buoyancy of the robot when the robot arm does not grasp the load, and g is the acceleration due to gravity;

[0091] Step S72, define matrix Vector b = [τ x τ y τ z +b0] T , the parameter vector to be estimated θ=[m F x F y F z ] T and arranging the load parameters of the manipulator into a parameter estimation equation of b=Aθ;

[0092] Step S73: Establish the inequality constraint {a xk , a yk , a zk}, expressed as,

[0093]

[0094] in, is the coordinate of the center of mass position of the robot arm in the base coordinate system {B1} when the robot arm does not grasp the load, is the second-order derivative of each degree of freedom of the base link at the kth time step in the world coordinate system {W}, z ik (i=1, 2, 3) represents the compensation result of the extended state observer for the lumped disturbance of each position degree of freedom of the manipulator at the kth time step;

[0095] Step S74: Determine the z according to the inequality constraint of the single time step k. ik The expression is:

[0096]

[0097] in, Represents the output result of the extended state observer at the kth time step The i-th dimension of

[0098] Step S75: Calculate the constraint conditions of the complex environmental disturbance as follows:

[0099]

[0100] Among them, m pmax is the maximum load mass that the robotic arm can grasp;

[0101] Step S76: Establish a nonlinear programming form for a single time step k:

[0102] min f(x)

[0103]

[0104] Where f(x) is the objective function of the nonlinear programming form, g i (x) and m are the inequality constraints and their number, respectively. j (x) and n are the equality constraints and their number respectively;

[0105] Step S77: the objective function of the nonlinear programming form is calculated at the iteration point x k Perform Taylor expansion near , and obtain the subform of the quadratic programming form, which is expressed as:

[0106]

[0107] Where Δx is the step vector, H k is the Hessian matrix approximation of the objective function of the nonlinear programming form, is the gradient value;

[0108] Step S78: Establish a quadratic programming form for the single time step k according to the subform of the quadratic programming form, which is expressed as:

[0109]

[0110] Where k is the kth time step, x0 is the starting point for solving the quadratic programming form, is the parameter θ to be estimated at the kth time step k The solution is θ0=[m0 0 0 0] T ;

[0111] According to the parameter θ to be estimated at the kth time step k The load mass is calculated from the solution

[0112] Furthermore, according to the parameter θ to be estimated at the kth time step k The load mass is calculated from the solution include:

[0113] Get the parameters to be estimated at the current moment

[0114] The estimated result at the k-1th time step is used as the starting point for solving the quadratic programming form at the kth time step, and The first dimension

[0115] according to Get the load mass

[0116] Compared with the prior art, the beneficial effect of the present invention lies in that the method of the present invention adopts integral sliding mode control as the basic motion controller of the underwater swimming manipulator, and on this basis introduces the output parameters of the integral sliding mode controller to alleviate the chattering phenomenon of the integral sliding mode; in order to reduce the impact of complex environment on the motion performance of the underwater swimming manipulator, an extended state observer is set to compensate for uncertainties such as the unmodeled dynamics and external disturbances of the manipulator. At the same time, in order to address the problem of control performance degradation caused by the dead zone characteristics of the thruster and considering the special case where the output value of the dead zone boundary is non-zero, a thrust distribution algorithm based on mixed integer linear programming is set, so that the thrust value can avoid falling into the dead zone range while meeting the thrust distribution requirements; in order to solve the problem of changes in the dynamic parameters of the underwater swimming manipulator after grabbing the load and the resulting degradation of control performance, the load mass based on recursive quadratic programming is calculated, thereby realizing real-time updating of the dynamic parameters of the manipulator and improving the motion stability of the underwater swimming manipulator.

[0117] Furthermore, the method of the present invention sets a preset underwater noise. Noise caused by water flow, marine biological activity, ship vibration, sound waves generated by other equipment such as sonar, etc. will mix with the transmitted signal, resulting in a decrease in signal strength and a decrease in signal-to-noise ratio, which in turn leads to a decrease in the accuracy of the detected data. By reducing the posture change speed of the robotic arm, the noise caused by the stirring of the robotic arm is suppressed, and by adjusting the data fusion range of the kinematic model and the dynamic model to improve the detection accuracy by increasing redundant information, the observation accuracy of the expanded state observer is increased.

[0118] Furthermore, the method of the present invention establishes a kinematic model and a dynamic model of the manipulator, considers the acceleration change of each positional degree of freedom of the base link in the world coordinate system {W}, and the influence of the load mass on the total mass of the manipulator, and introduces the torque τ, the load mass m p , and complex environmental disturbances F x 、F y 、F z , which can more accurately describe the dynamic behavior of the robot arm in a complex environment and improve the accuracy of the control of the robot arm.

[0119] Furthermore, the method of the present invention calculates the compensation output parameters of the extended state observer and the output parameters of the integral sliding mode controller and uses them as decision variables. Due to the particularity of the underwater swimming environment, such as the influence of factors such as water flow disturbance and underwater pressure on the motion performance of the manipulator, by compensating for uncertainties such as the unmodeled dynamics and external disturbances of the manipulator, and utilizing the robustness of the sliding mode control, the load mass is estimated by using the output of the observer, and the load mass information is separated from other disturbances. This copes with the dead zone characteristics of the thruster, thereby improving the adaptability and stability of the manipulator in complex underwater environments.

[0120] Furthermore, the method of the present invention introduces a thrust distribution algorithm of mixed integer linear programming. The present invention can accurately calculate the thrust output of the propeller and avoid the thrust value falling into the dead zone range, thereby improving the control accuracy and response speed of the robotic arm; the underwater swimming robotic arm mainly relies on the propeller to adjust its own posture when performing work tasks, but the dead zone characteristics of the propeller weaken its motion performance. The propulsion dead zone refers to the situation where the propeller cannot generate effective thrust within a certain input range, which increases the difficulty of motion control of the underwater swimming robotic arm. In order to address the problem of decreased control performance caused by the dead zone characteristics of the propeller, a thrust distribution algorithm based on mixed integer linear programming is set up, and by introducing integer auxiliary decision variables yi and zi, a mixed integer linear programming form is constructed and solved to obtain a thrust value that meets the thrust distribution requirements and avoids falling into the dead zone range, and then handles the special case where the dead zone boundary output value is non-zero, thereby enhancing the practicality and adaptability of the algorithm.

[0121] Furthermore, the method of the present invention calculates the load mass of the robotic arm. Since the dynamic characteristics of the underwater swimming robotic arm, such as mass distribution and inertia parameters, will change after grabbing the load, the motion balance of the underwater swimming robotic arm will be disturbed. By effectively estimating the load mass and dynamically adjusting the system dynamic parameters, the control accuracy and system stability are improved. In order to reduce the amount of calculation in the quadratic programming form and improve the accuracy of the parameter estimation results, the load mass estimation results under continuous time steps are iteratively optimized until the preset number of iterations is reached or the convergence conditions are met, thereby improving the accuracy of the continuous and precise estimation of the load mass and improving the motion stability and load adaptability of the robotic arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0122] Figure 1 This is an overall flow chart of a method for controlling an underwater swimming robotic arm based on load estimation according to an embodiment of the present invention;

[0123] Figure 2 Schematic diagram of a model of an underwater swimming manipulator according to a method for controlling an underwater swimming manipulator based on load estimation according to an embodiment of the present invention;

[0124] Figure 3 This is a logic flow chart of a method for controlling an underwater swimming manipulator based on load estimation according to an embodiment of the present invention;

[0125] Figure 4 Schematic diagram of changes in pitch and yaw joint angles of an underwater swimming manipulator arm according to a method for controlling an underwater swimming manipulator arm based on load estimation according to an embodiment of the present invention;

[0126] Figure 5 A comparison diagram of the position of an underwater swimming manipulator and an expected value according to a method for controlling an underwater swimming manipulator based on load estimation according to an embodiment of the present invention;

[0127] Figure 6 A comparison diagram of the posture of an underwater swimming manipulator and an expected value according to a method for controlling an underwater swimming manipulator based on load estimation according to an embodiment of the present invention;

[0128] Figure 7 A position error curve diagram of an underwater swimming manipulator according to a method for controlling an underwater swimming manipulator based on load estimation according to an embodiment of the present invention;

[0129] Figure 8 An error curve diagram of the underwater swimming manipulator posture of the underwater swimming manipulator control method based on load estimation according to an embodiment of the present invention;

[0130] Figure 9 Schematic diagram of the estimation results of the disturbance of each position degree of freedom by the extended state observer of the underwater swimming manipulator control method based on load estimation according to an embodiment of the present invention;

[0131] Figure 10 Schematic diagram of estimation results of disturbances of each posture degree of freedom by an extended state observer in a method for controlling an underwater swimming manipulator based on load estimation according to an embodiment of the present invention;

[0132] Figure 11 Thrust diagram of the thrusters 106, 107, 108, and 109 of the underwater swimming manipulator control method based on load estimation according to an embodiment of the present invention;

[0133] Figure 12 Thrust diagrams of the thrusters 110, 111, 112, and 113 of the underwater swimming manipulator control method based on load estimation according to an embodiment of the present invention;

[0134] Figure 13 A load mass variation diagram of the underwater swimming manipulator control method based on load estimation according to an embodiment of the present invention;

[0135] Description of the accompanying drawings: 106 - propeller 106, 107 - propeller 107, 108 - propeller 108, 109 - propeller 109, 110 - propeller 110, 111 - propeller 111, 112 - propeller 112, 113 - propeller 113. DETAILED DESCRIPTION

[0136] In order to make the objects and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with embodiments; it should be understood that the specific embodiments described herein are merely used to explain the present invention and are not intended to limit the present invention.

[0137] The preferred embodiments of the present invention are described below with reference to the accompanying drawings. It should be understood by those skilled in the art that these embodiments are only used to explain the technical principles of the present invention and are not intended to limit the scope of protection of the present invention.

[0138] It should be noted that, in the description of the present invention, terms such as "up", "down", "left", "right", "inside", and "outside" indicating directions or positional relationships are based on the directions or positional relationships shown in the accompanying drawings. This is only for the convenience of description and does not indicate or imply that the device or element must have a specific orientation, be constructed and operated in a specific orientation. Therefore, it cannot be understood as a limitation on the present invention.

[0139] Furthermore, it should be noted that, in the description of the present invention, unless otherwise expressly specified or limited, the terms "mounted," "connected," and "connected" should be understood in a broad sense. For example, they may refer to fixed connections, detachable connections, or integral connections; mechanical connections or electrical connections; direct connections or indirect connections through an intermediate medium; and internal communication between two components. Those skilled in the art will understand the specific meanings of the above terms in the present invention based on specific circumstances.

[0140] See also Figure 1 、 Figure 2 、 Figure 3 As shown in the figure, they are respectively an overall flow chart of the underwater swimming manipulator control method based on load estimation according to an embodiment of the present invention, a model schematic diagram of the underwater swimming manipulator, and a logic flow chart. The present invention provides an underwater swimming manipulator control method based on load estimation, comprising:

[0141] Step S1, establishing a kinematic model and a dynamic model of the robotic arm according to the motion data of the robotic arm;

[0142] Step S2, determining whether the degree of environmental interference meets the requirements based on the underwater noise, and determining an adjustment method if it does not meet the requirements, including adjusting the posture change speed of the manipulator or adjusting the data fusion range of the kinematic model and the dynamic model;

[0143] Step S3, obtaining the current position and posture of the robotic arm according to the adjustment method;

[0144] Step S4, calculating the output parameters of the integral sliding mode controller and the compensation output parameters of the extended state observer based on the kinematic model, the dynamic model and the current posture;

[0145] Step S5, calculating the torque of the manipulator according to the output parameters of the integral sliding mode controller and the compensation output coefficient of the extended state observer;

[0146] Step S6, performing thrust distribution on a plurality of thrusters of the manipulator according to the torque calculation to obtain a plurality of thrust values;

[0147] Step S7, calculating the load mass of the robotic arm according to the compensation output parameters of the extended state observer;

[0148] Step S8, adjusting the robotic arm to an updated posture according to the thrust value and the load mass of the robotic arm;

[0149] Step S9: If the updated posture is inconsistent with the target posture, repeat steps S3 to S8 until the robotic arm reaches the target posture.

[0150] In implementation, the method of the present invention adopts integral sliding mode control as the basic motion controller of the underwater swimming manipulator, and on this basis introduces the output parameters of the integral sliding mode controller to alleviate the chattering phenomenon of the integral sliding mode; in order to reduce the impact of complex environment on the motion performance of the underwater swimming manipulator, an extended state observer is set to compensate for uncertainties such as the unmodeled dynamics and external disturbances of the manipulator. At the same time, in order to address the problem of control performance degradation caused by the dead zone characteristics of the thruster and considering the special case of non-zero output value at the dead zone boundary, a thrust distribution algorithm based on mixed integer linear programming is set, so that the thrust value can avoid falling into the dead zone range while meeting the thrust distribution requirements; in order to solve the problem of changes in the dynamic parameters of the underwater swimming manipulator after grabbing the load and the resulting degradation of control performance, the load mass based on recursive quadratic programming is calculated, thereby realizing real-time update of the dynamic parameters of the manipulator, and improving the motion stability of the underwater swimming manipulator.

[0151] Specifically, the step S2 includes:

[0152] Obtaining underwater noise at a position of the extended state observer;

[0153] Comparing the underwater noise with a preset first underwater noise and a preset second underwater noise respectively;

[0154] If the underwater noise is greater than the preset first underwater noise, it is determined that the environmental interference level does not meet the requirement;

[0155] When the underwater noise is greater than the preset second underwater noise, reducing the posture change speed of the robotic arm;

[0156] When the underwater noise is greater than the preset first underwater noise and less than or equal to the preset second underwater noise, preliminarily determining that the accuracy of the expanded state observer does not meet the requirement, and obtaining a sensitivity change of the expanded state observer within a single detection cycle;

[0157] If the sensitivity change is greater than the preset change, it is determined that the accuracy of the extended state observer does not meet the requirements, and the data fusion range of the kinematic model and the dynamic model is increased.

[0158] Among them, the posture change speed of the robotic arm is negatively correlated with the underwater noise, the data fusion range is positively correlated with the sensitivity change; the sensitivity change of the expanded state observer within a single detection cycle is the difference between the sensitivity at the beginning of the detection cycle and the sensitivity at the end of the detection cycle.

[0159] Specifically, underwater noise is detected by an underwater noise analyzer installed at the base of the robotic arm.

[0160] Specifically, the sensitivity is the voltage fluctuation amplitude of the extended state observer.

[0161] Specifically, the posture change speed of the robot arm is the ratio of the resultant vector of the movement of the robot arm to the unit time.

[0162] Specifically, the data fusion range is the amount of overlapping data of the kinematic model and the dynamic model.

[0163] Specifically, the general value range of the preset first underwater noise is [200 Hz, 240 Hz], and the general value range of the preset second underwater noise is [300 Hz, 350 Hz].

[0164] Preferably, the preset frequency of the first underwater noise is 220 Hz, and the preset frequency of the second underwater noise is 310 Hz.

[0165] Specifically, the general value range of the preset change amount is [2mV, 2.4mV], and the preferred embodiment of the preset change amount is 2.2mV.

[0166] During implementation, the method of the present invention sets a preset underwater noise. Noise caused by water flow, marine biological activities, ship vibrations, sound waves generated by other equipment such as sonar, etc. will mix with the transmitted signal, resulting in a decrease in signal strength and a decrease in signal-to-noise ratio, which in turn leads to a decrease in the accuracy of the detected data. By reducing the posture change speed of the robotic arm, the noise caused by the stirring of the robotic arm is suppressed. By adjusting the data fusion range of the kinematic model and the dynamic model to improve the detection accuracy by increasing redundant information, the observation accuracy of the expanded state observer is increased.

[0167] Specifically, the kinematic model of the robotic arm is established, including:

[0168] Define the state vector of the manipulator in the world coordinate system {W}, define the velocity vector of the manipulator in the base coordinate system {B1}, and define the motion rotation of each link of the manipulator;

[0169] The position and posture of each link {Bi} of the manipulator in the world coordinate system {W} are calculated based on the state vector and the velocity vector. The position and posture are expressed as follows:

[0170]

[0171] Among them, H i is the position of connecting rod i, and is the rotation matrix and position vector of the i-th link coordinate system {Bi} relative to the world coordinate system {W}, 0m×n Represents an m-by-n zero matrix.

[0172] Specifically, the world coordinate system {W} and the link coordinate system {Bi} of the underwater swimming manipulator are established based on the right-hand rule.

[0173] Specifically, the establishment of the kinetic model includes:

[0174] Establishing a generalized inertia matrix of the robotic arm;

[0175] Establishing an expression of the dynamic model according to the generalized inertia matrix;

[0176] Calculating the torque according to the expression of the dynamic model;

[0177] Wherein, the expression of the kinetic model is: The expression of the moment is τ=M(u AdaISMC +u ESO ),

[0178] M is the generalized inertia matrix, x1 and x2 are the state vector and velocity vector in the base coordinate system {B1}, τ is the torque, and N includes the uncertainty vector of the kinematic model and the external disturbance factor vector; u AdaISMC is the output parameter of the adaptive integral sliding mode controller, u ESO is the compensation output parameter of the extended state observer.

[0179] In implementation, the method of the present invention establishes a kinematic model and a dynamic model of the robotic arm, considers the acceleration changes of each position degree of freedom of the base link in the world coordinate system {W}, and the influence of the load mass on the total mass of the robotic arm, and introduces the torque τ and the load masses Fx, Fy, and Fz. It can more accurately describe the dynamic behavior of the robotic arm in a complex environment, thereby improving the accuracy of the control of the robotic arm.

[0180] Specifically, the calculation of the compensation output parameter of the extended state observer includes:

[0181] Define the extended state vector z=[z1z2z3] T =[x1 x2 N] T , where z3 includes the extended state quantity of unknown external disturbance and model uncertainty;

[0182] The state observation value vector of the extended state observer is designed according to the extended state vector:

[0183]

[0184] in, is the state observation value vector; ρ1, ρ2, ρ3 are the gains of the extended state observer;

[0185] Determine the compensation output parameter of the extended state observer based on the state observation value vector

[0186] Specifically, the calculation of the output parameters of the integral sliding mode controller includes:

[0187] The base link tracking error of the manipulator is defined as e = ξ - ξ d , where ξ d is the desired position and angle of each joint of the base link in the world coordinate system {W};

[0188] The integral sliding surface is determined according to the desired posture and the desired angles of each joint: Where c1 is the positive definite diagonal matrix of the proportional gain of the integrated sliding mode surface, and c2 is the positive definite diagonal matrix of the integral gain of the integrated sliding mode surface;

[0189] The derivative of the integral sliding surface is obtained: Right now

[0190] The output parameters of the integral sliding mode controller are determined based on the derived integral sliding mode surface:

[0191] Among them, K s is the diagonal gain matrix sgn(*) is a sign function, and its specific form is: Switching gain matrix adjusted for the update law

[0192] The update law is designed as follows:

[0193]

[0194] in, for The i-th value on the diagonal, is the rate of change of the adaptive gain, and χ i is a positive real number tending to zero; ε i is the boundary layer;

[0195] When|s i |<ε i When , it is determined that the robotic arm reaches the sliding surface.

[0196] Specifically, step S5 includes:

[0197] Step S51, defining control input parameters of the actuator of the robotic arm;

[0198]

[0199] Among them, u thr Indicates n t The control input parameters of the thrusters of the manipulator, u q Represents the control input parameters of n-1 joint motors;

[0200] Step S52, calculating the torque of the robotic arm according to the control input parameters of the actuator;

[0201] Wherein, the torque of the robotic arm is τ=Tu;

[0202] Where T is the control allocation matrix, specifically,

[0203]

[0204] Wherein, B is the thrust distribution matrix of the propeller, Β joint is the joint torque distribution matrix of the robotic arm;

[0205] Step S53: Determine the first six dimensions of the torque τ according to the control allocation matrix as τ thr =Bu thr , where u thr is the thrust value of the propeller;

[0206] Step S54, solving the first six dimensions of the torque τ to obtain a general solution for the thrust value of the propeller;

[0207] Wherein, the general solution is: thr =a1ξ1+a2ξ2+ξ0,

[0208] in, is a homogeneous linear system of equations The basic solution system, a1, a2 are the coefficients corresponding to ξ1, ξ2, ξ0 is the non-homogeneous linear equation system τ thr =Bu thr The special solution of .

[0209] In implementation, the method of the present invention calculates the compensation output parameters of the extended state observer and the output parameters of the integral sliding mode controller and uses them as decision variables. Due to the particularity of the underwater swimming environment, such as the influence of factors such as water flow disturbance and underwater pressure on the motion performance of the manipulator, by compensating for uncertainties such as the unmodeled dynamics and external disturbances of the manipulator, and utilizing the robustness of the sliding mode control, the load mass is estimated by using the output of the observer, and the load mass information is separated from other disturbances. This copes with the dead zone characteristics of the thruster, thereby improving the adaptability and stability of the manipulator in complex underwater environments.

[0210] Specifically, step S6 includes:

[0211] Step S61: Establishing the objective function of the thrust distribution model for the robotic arm:

[0212]

[0213] Among them, u thri =a1ξ 1i +a2ξ 2i +ξ 0i represents the thrust value of the i-th thruster;

[0214] Step S62: introducing auxiliary variable u into the objective function i (i=1,2,…,n), and add constraint u i ≥|u thri |;

[0215] Step S63: Determine the final form of the objective function according to the constraint conditions:

[0216] Step S64, set dead zone constraint u thri ≤d - oru thri ≥d + , where d + >0 is the forward dead zone of the thruster, d - <0 is the reverse dead zone of the propeller;

[0217] Step S65: When the thrust value is within the range of d - ≤u thri ≤d + When , the thrust value actually output by the propeller is zero;

[0218] Step S66: introducing an integer auxiliary decision variable y for each thrust value i and z i ,

[0219] in, i=1,2,…,n, and y i +z i =1;

[0220] Step S67, according to the integer auxiliary decision variable y i and z i Identify linear constraints:

[0221]

[0222] Where Ω≥10 6 ;

[0223] Step S68, introduce the standard form of linear programming, expressed as:

[0224]

[0225] stx≤x c orx≥x c ,

[0226] Among them, J is the objective function of linear programming, x is the vector of decision variables, c is the coefficient vector of decision variables, x c is an inequality constraint;

[0227] Step S69, according to the integer auxiliary decision variable y i and z i , and the standard form of linear programming constructs the mixed integer linear programming form and is expressed as:

[0228]

[0229]

[0230] Among them, x Z is x c Integer decision variables in ;

[0231] Step S610: construct an integer linear programming form of the propeller according to the mixed integer linear programming form, and the expression is:

[0232]

[0233] Solve the integer linear programming form of the propeller and obtain the state quantity a respectively 1_optimal and a 2_optimal ;

[0234] Step S611: 1_optimal and the a 2_optimalSubstitute the general solution expression of the thrust value of the propeller and obtain the thrust value u of each propeller of the manipulator thr =a 1_optimal ξ1+a 2_optimal ξ2+ξ0.

[0235] In implementation, the method of the present invention introduces a thrust distribution algorithm of mixed integer linear programming. The present invention can accurately calculate the thrust output of the propeller and avoid the thrust value falling into the dead zone range, thereby improving the control accuracy and response speed of the robotic arm; the underwater swimming robotic arm mainly relies on the propeller to adjust its own posture when performing work tasks, but the dead zone characteristics of the propeller weaken its motion performance. The propulsion dead zone refers to the situation where the propeller cannot generate effective thrust within a certain input range, which increases the difficulty of motion control of the underwater swimming robotic arm. In order to address the problem of decreased control performance caused by the dead zone characteristics of the propeller, a thrust distribution algorithm based on mixed integer linear programming is set up, and by introducing integer auxiliary decision variables yi and zi, a mixed integer linear programming form is constructed and solved to obtain a thrust value that meets the thrust distribution requirements and avoids falling into the dead zone range, and then handles the special case where the dead zone boundary output value is non-zero, thereby enhancing the practicality and adaptability of the algorithm.

[0236] Specifically, step S7 includes:

[0237] Step S71, establishing the load parameters of the robotic arm according to the dynamic model;

[0238]

[0239] Where m = m0 + m p is the total mass of the manipulator, m0 is the mass of the manipulator when it is not grabbing the load, and m p is the load mass, is the acceleration of each degree of freedom of the base link in the world coordinate system {W}, τ x , τ y , τ z is the first three dimensions of the moment τ, F x 、F y 、F z is the complex environmental disturbance, b0 is the buoyancy of the robot when the robot arm does not grasp the load, and g is the acceleration due to gravity;

[0240] Step S72, define matrix Vector b = [τ x τ y τ z +b0] T , the parameter vector to be estimated θ=[m F x F yF z ] T and arranging the load parameters of the manipulator into a parameter estimation equation of b=Aθ;

[0241] Step S73: Establish the inequality constraint {a xk , a yk , a zk}, expressed as,

[0242]

[0243] in, is the coordinate of the center of mass position of the robot arm in the base coordinate system {B1} when the robot arm does not grasp the load, is the second-order derivative of each degree of freedom of the base link at the kth time step in the world coordinate system {W}, z ik (i=1, 2, 3) represents the compensation result of the extended state observer for the lumped disturbance of each position degree of freedom of the manipulator at the kth time step;

[0244] Step S74: Determine the z according to the inequality constraint of the single time step k. ik The expression is:

[0245]

[0246] in, Represents the output result of the extended state observer at the kth time step The i-th dimension of

[0247] Step S75: Calculate the constraint conditions of the complex environmental disturbance as follows:

[0248]

[0249] Among them, m pmax is the maximum load mass that the robotic arm can grasp;

[0250] Step S76: Establish a nonlinear programming form for a single time step k:

[0251]

[0252] Where f(x) is the objective function of the nonlinear programming form, g i (x) and m are the inequality constraints and their number, respectively. j (x) and n are the equality constraints and their number respectively;

[0253] Step S77: the objective function of the nonlinear programming form is calculated at the iteration point x kPerform Taylor expansion near , and obtain the subform of the quadratic programming form, which is expressed as:

[0254]

[0255] Where Δx is the step vector, H k is the Hessian matrix approximation of the objective function of the nonlinear programming form, is the gradient value;

[0256] Step S78: Establish a quadratic programming form for the single time step k according to the subform of the quadratic programming form, which is expressed as:

[0257]

[0258] Where k is the kth time step, x0 is the starting point for solving the quadratic programming form, is the parameter θ to be estimated at the kth time step k The solution is θ0=[m0 0 0 0] T ;

[0259] According to the parameter θ to be estimated at the kth time step k The load mass is calculated from the solution

[0260] Specifically, according to the parameter θ to be estimated at the kth time step k The load mass is calculated from the solution include:

[0261] Get the parameters to be estimated at the current moment

[0262] The estimated result at the k-1th time step is used as the starting point for solving the quadratic programming form at the kth time step, and The first dimension

[0263] according to Get the load mass

[0264] In implementation, the method of the present invention calculates the load mass of the robotic arm. Since the dynamic characteristics of the underwater swimming robotic arm, such as mass distribution and inertia parameters, will change after grabbing the load, the motion balance of the underwater swimming robotic arm will be disturbed. By effectively estimating the load mass and dynamically adjusting the system dynamic parameters, the control accuracy and system stability are improved. In order to reduce the amount of calculation in the quadratic programming form and improve the accuracy of the parameter estimation results, the load mass estimation results under continuous time steps are iteratively optimized until the preset number of iterations is reached or the convergence conditions are met, thereby improving the accuracy of the continuous and precise estimation of the load mass and improving the motion stability and load adaptability of the robotic arm.

[0265] Example 1, as Figure 4 、 Figure 5 、 Figure 6 、 Figure 7 、 Figure 8 、 Figure 9 、 Figure 10 、 Figure 11 、 Figure 12 as well as Figure 13 As shown, they are respectively a schematic diagram of the change of the pitch and yaw joint angles of the underwater swimming manipulator arm of the underwater swimming manipulator control method based on load estimation in an embodiment of the present invention, a comparison diagram of the underwater swimming manipulator arm position and the expected value, a comparison diagram of the underwater swimming manipulator arm posture and the expected value, a curve diagram of the underwater swimming manipulator arm position error, a curve diagram of the error of the underwater swimming manipulator arm posture, a schematic diagram of the estimation results of the extended state observer for the disturbance of each position degree of freedom, a schematic diagram of the estimation results of the extended state observer for the disturbance of each attitude degree of freedom, a thrust diagram of thrusters 106, 107, 108 and 109, a thrust diagram of thrusters 110, 111, 112 and 113, and a load mass change diagram.

[0266] In order to verify the effectiveness of the solution proposed in this application, a simulation experiment of anti-disturbance control and load estimation of an underwater swimming manipulator after grabbing a load under complex environmental disturbances was carried out on the MATLAB simulation platform.

[0267] Specifically, the task of the underwater swimming manipulator is to start moving at the position of (-2m, -2m, -2m) in the world coordinate system at t = 0s, move to the origin of the world coordinate system at t = 40s, and remain stable at this position until t = 60s. During the movement, the base posture always remains (0rad, 0rad, 0rad). The underwater swimming manipulator grips the load, and the load mass is preset to 10kg; the pitch and yaw joints start to move along the set angle from t = 40s, such as Figure 4 As shown, it stops at t=60s. The following disturbances are introduced to the underwater manipulator to simulate the influence of complex underwater environment:

[0268]

[0269] Where, dist1 = [5 10 10 0.5 1 1] T , dist2=[1 1 1 0.1 0.1 0.1] T , t is the simulation time.

[0270] Specifically, the integral sliding mode controller parameters include:

[0271] c1=diag(1,0.3,0.5,2,0.8,0.6)

[0272] c2=diag(0.6,0.3,0.5,1,0.8,0.8)

[0273] K s =diag(0.2,0.2,0.2,8,8,8)

[0274]

[0275] χ=[χ1 K χ i ] T =[1 -20 1 -20 1 -20 1 -20 1 -20 1 -20 ] T

[0276] ε=diag(ε1,...,ε i )=diag(0.01,0.01,0.01,0.1,0.1,0.1)

[0277] Where diag(*) represents a diagonal matrix.

[0278] Specifically, the extended state observer parameters include:

[0279]

[0280] Wherein, w0 is the observer bandwidth, which is set to 15; I6 represents the 6×6 unit matrix.

[0281] Specifically, the thrust value range parameter is Ω=10 6 , d + =2.59,d - =-1.59.

[0282] Specifically, the load mass is m0=122.2, m pmax=10.

[0283] Specifically, the thrusters 106 , 107 , 108 , 109 , 110 , 111 , 112 , and 113 operate according to the thrust values.

[0284] Specifically, the pitch and yaw joint angles of the manipulator arm during the movement process of the manipulator arm performing actions according to the thrust value are analyzed, the position of the manipulator arm is compared with the expected value, the posture of the manipulator arm is compared with the expected value, the position of the manipulator arm is compared with the expected position and the error results are analyzed, the posture of the manipulator arm is compared with the expected posture and the error results are analyzed, the estimation results of the extended state observer for the disturbance of each position degree of freedom are calculated, the estimation results of the extended state observer for the disturbance of each posture degree of freedom are calculated, and the comparison results of the load mass of the manipulator arm and the estimated mass in the process of applying load to the manipulator arm are analyzed.

[0285] like Figure 5 and Figure 6 As shown in the figure, the underwater mobile manipulator can still achieve anti-disturbance control after grabbing the load, and the control accuracy is good. Figure 9 and Figure 10 The extended state observer is shown to accurately estimate the uncertainties; Figure 11 and Figure 12 As shown in the figure, the thrust of each thruster avoids the dead zone and no obvious buffeting phenomenon occurs; Figure 13 This proves that the solution proposed in this application can achieve effective estimation of load mass.

[0286] In summary, the method of the present invention improves the operational stability of the robotic arm.

[0287] Thus far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it will be readily understood by those skilled in the art that the scope of protection of the present invention is obviously not limited to these specific embodiments. Without departing from the principles of the present invention, those skilled in the art may make equivalent changes or substitutions to the relevant technical features, and the technical solutions after such changes or substitutions will fall within the scope of protection of the present invention.

Claims

1. A method for controlling an underwater swimming manipulator based on load estimation, characterized in that: include: Step S1, establishing a kinematic model and a dynamic model of the robotic arm according to the motion data of the robotic arm; Step S2, determining whether the degree of environmental interference meets the requirements based on the underwater noise, and determining an adjustment method if it does not meet the requirements, including adjusting the posture change speed of the manipulator or adjusting the data fusion range of the kinematic model and the dynamic model; Step S3, obtaining the current position and posture of the robotic arm according to the adjustment method; Step S4, calculating the output parameters of the integral sliding mode controller and the compensation output parameters of the extended state observer based on the kinematic model, the dynamic model and the current posture; Step S5, calculating the torque of the manipulator according to the output parameters of the integral sliding mode controller and the compensation output coefficient of the extended state observer; Step S6, performing thrust distribution on a plurality of thrusters of the manipulator according to the torque calculation to obtain a plurality of thrust values; Step S7, calculating the load mass of the robotic arm according to the compensation output parameters of the extended state observer; Step S8, adjusting the robotic arm to an updated posture according to the thrust value and the load mass of the robotic arm; Step S9: If the updated posture is inconsistent with the target posture, repeat steps S3 to S8 until the robotic arm reaches the target posture.

2. The underwater swimming manipulator control method based on load estimation according to claim 1 is characterized in that: The step S2 includes: Obtaining underwater noise at a position of the extended state observer; Comparing the underwater noise with a preset first underwater noise and a preset second underwater noise respectively; If the underwater noise is greater than the preset first underwater noise, it is determined that the environmental interference level does not meet the requirements; When the underwater noise is greater than the preset second underwater noise, reducing the posture change speed of the robotic arm; When the underwater noise is greater than the preset first underwater noise and less than or equal to the preset second underwater noise, preliminarily determining that the accuracy of the expanded state observer does not meet the requirement, and obtaining a sensitivity change of the expanded state observer within a single detection cycle; If the sensitivity change is greater than the preset change, it is determined that the accuracy of the extended state observer does not meet the requirements, and the data fusion range of the kinematic model and the dynamic model is increased. Among them, the posture change speed of the robotic arm is negatively correlated with the underwater noise, the data fusion range is positively correlated with the sensitivity change; the sensitivity change of the expanded state observer within a single detection cycle is the difference between the sensitivity at the beginning of the detection cycle and the sensitivity at the end of the detection cycle.

3. The underwater swimming manipulator control method based on load estimation according to claim 2 is characterized in that: The kinematic model of the robotic arm is established, comprising: Define the state vector of the manipulator in the world coordinate system {W}, define the velocity vector of the manipulator in the base coordinate system {B1}, and define the motion rotation of each link of the manipulator; The position and posture of each link {Bi} of the manipulator in the world coordinate system {W} are calculated based on the state vector and the velocity vector. The position and posture are expressed as follows: Among them, H i is the position of connecting rod i, and is the rotation matrix and position vector of the i-th link coordinate system {Bi} relative to the world coordinate system {W}, 0 m×n Represents an m-by-n zero matrix.

4. The underwater swimming manipulator control method based on load estimation according to claim 3 is characterized in that: The kinetic model is established, comprising: Establishing a generalized inertia matrix of the robotic arm; Establishing an expression of the dynamic model according to the generalized inertia matrix; Calculating the torque according to the expression of the dynamic model; Wherein, the expression of the kinetic model is: The expression of the moment is τ=M(u AdaISMC +u ESO ), M is the generalized inertia matrix, x1 and x2 are the state vector and velocity vector in the base coordinate system {B1}, τ is the torque, and N includes the uncertainty vector of the kinematic model and the external disturbance factor vector; u AdaISMC is the output parameter of the adaptive integral sliding mode controller, u ESO is the compensation output parameter of the extended state observer.

5. The underwater swimming manipulator control method based on load estimation according to claim 4 is characterized in that: The calculating of the compensation output parameter of the extended state observer includes: Define the extended state vector z as z = [z1 z2 z3] T =[x1 x2 N] T , where z3 includes the extended state quantity of unknown external disturbance and model uncertainty; The state observation value vector of the extended state observer is designed according to the extended state vector: in, is the state observation value vector; ρ1, ρ2, ρ3 are the gains of the extended state observer, and u is the control input parameter of the actuator of the manipulator; Determine the compensation output parameter of the extended state observer based on the state observation value vector 6. The underwater swimming manipulator control method based on load estimation according to claim 5 is characterized in that: The calculating the output parameter of the integral sliding mode controller includes: The base link tracking error of the manipulator is defined as e = ξ - ξ d , where ξ d is the desired position and angle of each joint of the base link in the world coordinate system {W}, and ξ is the pitch and yaw joint angle of the manipulator during the movement of the manipulator according to the thrust value; The integral sliding surface is determined according to the desired posture and the desired angles of each joint: Where c1 is the positive definite diagonal matrix of the proportional gain of the integrated sliding mode surface, and c2 is the positive definite diagonal matrix of the integral gain of the integrated sliding mode surface; The derivative of the integral sliding surface is obtained: Right now The output parameters of the integral sliding mode controller are determined based on the derived integral sliding mode surface: Among them, K s is the diagonal gain matrix sgn(*) is a sign function, and its specific form is: Switching gain matrix adjusted for the update law The update law is designed as follows: in, for The i-th value on the diagonal, is the rate of change of the adaptive gain, and χ i is a positive real number tending to zero; ε i is the boundary layer; When|s i |<ε i When , it is determined that the robotic arm reaches the sliding surface.

7. The underwater swimming manipulator control method based on load estimation according to claim 6 is characterized in that: The step S5 comprises: Step S51, defining control input parameters of the actuator of the robotic arm; Among them, u thr Indicates n t The control input parameters of the thrusters of the manipulator, u q Represents the control input parameters of n-1 joint motors; Step S52, calculating the torque of the robotic arm according to the control input parameters of the actuator; Wherein, the torque of the robotic arm is τ=Tu; Where T is the control allocation matrix, specifically, Wherein, B is the thrust distribution matrix of the propeller, Β joint is the joint torque distribution matrix of the robotic arm; Step S53: Determine the first six dimensions of the torque τ according to the control allocation matrix as τ thr =Bu thr , where u thr is the thrust value of the propeller; Step S54, solving the first six dimensions of the torque τ to obtain a general solution for the thrust value of the propeller; Wherein, the general solution is: thr =a1ξ1+a2ξ2+ξ0, in, is a homogeneous linear system of equations The basic solution system, a1, a2 are the coefficients corresponding to ξ1, ξ2, ξ0 is the non-homogeneous linear equation system τ thr =Bu thr The special solution of .

8. The underwater swimming manipulator control method based on load estimation according to claim 7 is characterized in that: The step S6 comprises: Step S61: Establishing the objective function of the thrust distribution model for the robotic arm: Among them, u thri =a1ξ 1i +a2ξ 2i +ξ 0i represents the thrust value of the i-th thruster; Step S62: introducing auxiliary variable u into the objective function i (i=1,2,…,n), and add constraint u i ≥|u thri |; Step S63: Determine the final form of the objective function according to the constraint conditions: Step S64, set dead zone constraint u thri ≤d - or u thri ≥d + , where d + >0 is the forward dead zone of the thruster, d - <0 is the reverse dead zone of the propeller; Step S65: When the thrust value is within the range of d - ≤u thri ≤d + When , the thrust value actually output by the propeller is zero; Step S66: introducing an integer auxiliary decision variable y for each thrust value i and z i , in, Step S67, according to the integer auxiliary decision variable y i and z i Identify linear constraints: Where Ω≥10 6 ; Step S68, introduce the standard form of linear programming, expressed as: s.t.x≤x c orx≥x c , Among them, J is the objective function of linear programming, x is the vector of decision variables, c is the coefficient vector of decision variables, x c is an inequality constraint; Step S69, according to the integer auxiliary decision variable y i and z i , and the standard form of linear programming constructs the mixed integer linear programming form and is expressed as: Among them, x Z is x c Integer decision variables in ; Step S610: construct the mixed integer linear programming form of the propeller according to the mixed integer linear programming form, and the expression is: Solve the mixed integer linear programming form of the thruster and obtain the optimal state quantity a 1_optimal and a 2_optimal ; Step S611: 1_optimal and the a 2_optimal Substitute the general solution expression of the thrust value of the propeller and obtain the thrust value u of each propeller of the manipulator thr =a 1_optimal ξ1+a 2_optimal ξ2+ξ0.

9. The underwater swimming manipulator control method based on load estimation according to claim 8, characterized in that: The step S7 includes: Step S71, establishing the load parameters of the robotic arm according to the dynamic model; Where m = m0 + m p is the total mass of the manipulator, m0 is the mass of the manipulator when it is not grabbing the load, and m p is the load mass, is the acceleration of each degree of freedom of the base link in the world coordinate system {W}, τ x , τ y , τ z is the first three dimensions of the moment τ, F x 、F y 、F z is the complex environmental disturbance, b0 is the buoyancy of the robot when the robot arm does not grasp the load, and g is the acceleration due to gravity; Step S72, define matrix Vector b = [τ x τ y τ z +b0] T , the parameter vector to be estimated θ=[m F x F y F z ] T and arranging the load parameters of the manipulator into a parameter estimation equation of b=Aθ; Step S73: Establish the inequality constraint {a xk , a yk , a zk }, expressed as, in, is the coordinate of the center of mass position of the robot arm in the base coordinate system {B1} when the robot arm does not grasp the load, is the second-order derivative of each degree of freedom of the base link at the kth time step in the world coordinate system {W}, z ik (i=1, 2, 3) represents the compensation result of the extended state observer for the lumped disturbance of each position degree of freedom of the manipulator at the kth time step; Step S74: Determine the z according to the inequality constraint of the single time step k. ik The expression is: in, Represents the output result of the extended state observer at the kth time step The i-th dimension of Step S75: Calculate the constraint conditions of the complex environmental disturbance as follows: Among them, m pmax is the maximum load mass that the robotic arm can grasp; Step S76: Establish a nonlinear programming form for a single time step k: min f(x) Where f(x) is the objective function of the nonlinear programming form, g i (x) and m are the inequality constraints and their number, respectively. j (x) and n are the equality constraints and their number respectively; Step S77: the objective function of the nonlinear programming form is calculated at the iteration point x k Perform Taylor expansion near , and obtain the subform of the quadratic programming form, which is expressed as: Where Δx is the step vector, H k is the Hessian matrix approximation of the objective function of the nonlinear programming form, is the gradient value; Step S78: Establish a quadratic programming form for the single time step k according to the subform of the quadratic programming form, which is expressed as: Where k is the kth time step, x0 is the starting point for solving the quadratic programming form, is the parameter θ to be estimated at the kth time step k The solution is θ0=[m0 0 0 0] T ; According to the parameter θ to be estimated at the kth time step k The load mass is calculated from the solution 10. The underwater swimming manipulator control method based on load estimation according to claim 9, characterized in that: According to the parameter θ to be estimated at the kth time step k The load mass is calculated from the solution include: Get the parameters to be estimated at the current moment The estimated result at the k-1th time step is used as the starting point for solving the quadratic programming form at the kth time step, and The first dimension according to Get the load mass

Images