Underwater swimming mechanical arm control method based on load estimation

Abstract

The invention relates to the technical field of mechanical arm control, in particular to an underwater swimming mechanical arm control method based on load estimation, which comprises the following steps: respectively establishing a kinematics model and a dynamics model of a mechanical arm according to motion data of the mechanical arm; judging whether the environmental interference degree meets requirements or not according to the underwater noise, and determining an adjustment mode when the environmental interference degree does not meet the requirements; obtaining the pose at the current moment; respectively calculating an output parameter of the integral sliding mode controller and a compensation output parameter of the expansion state observer; calculating the torque of the mechanical arm; calculating a plurality of thrust values of a plurality of propellers of the mechanical arm; calculating the load mass of the mechanical arm; and the pose of the mechanical arm is updated according to the thrust value and the load mass of the mechanical arm, and the updated pose is obtained. The operation stability of the mechanical arm is improved, and the control accuracy is improved.

Description

Technical Field

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

[0002] In the prior art, underwater mobile manipulators mainly rely on thrusters to adjust their own posture when performing operating tasks. However, the dead zone characteristics of the thrusters may weaken their motion performance. The thruster dead zone refers to the situation where the thruster cannot generate effective thrust within a certain input range, which greatly increases the difficulty of motion control of the underwater mobile manipulator. Therefore, studying the thruster dead zone compensation problem of the underwater mobile manipulator is the key to improving its operating performance in complex marine environments. After the underwater mobile manipulator grabs the load, its dynamic characteristics such as the mass distribution and inertia parameters of the manipulator will change, which may interfere with the motion balance of the underwater mobile manipulator. For the anti-disturbance control problem of the underwater robot after grabbing the load, the existing scheme design includes but is 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 Gaussian and artificial potential fields on the basis of the 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 speed 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, and the second-level path planning is based on the first-level path planning. The model is iterated several times to reduce energy consumption to obtain the energy-optimal underwater swimming manipulator base path planning 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 robot arm control method based on load estimation, so as to overcome the problem in the prior art that the underwater swimming robot arm is difficult to maintain motion stability under complex environmental disturbances due to its light base, multiple joints and other characteristics.

[0005] To achieve the above object, the present invention provides an underwater swimming manipulator control method based on load estimation, comprising:

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

[0007] Step S2, determining whether the degree of environmental interference meets the requirements according to the underwater noise, and determining an adjustment method when 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, respectively calculating output parameters of an integral sliding mode controller and compensation output parameters of an extended state observer based on the kinematic model, the dynamic model and the current posture;

[0010] Step S5, calculating the torque of the robot arm 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 parameter 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 the steps S3 to S8 until the robot arm reaches the target posture.

[0015] Furthermore, the step S2 comprises:

[0016] Acquiring underwater noise at the 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 the 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 the 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 comprises:

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

[0025] The position and posture of each link {Bi} of the robot arm in the world coordinate system {W} are calculated according to the state vector and the velocity vector. The position and posture are expressed as:

[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 matrix of zeros.

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

[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] Among them, 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] Further, 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 as follows:

[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] Further, the calculating the output parameter 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 surface, and c2 is the positive definite diagonal matrix of the integral gain of the integrated sliding surface;

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

[0044] The output parameter of the integral sliding mode controller is determined according to 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 , it is determined that the robotic arm reaches the sliding surface.

[0050] Furthermore, the step S5 comprises:

[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 mechanical 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, determining 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 moment τ to obtain a general solution for the thrust value of the propeller;

[0061] Among them, 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 comprises:

[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 an auxiliary variable u into the objective function i (i=1,2,…,n), and add constraint u i ≥|u thri |;

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

[0069] Step S64, setting the dead zone constraint u thri ≤d-oru thri ≥d + , where d + >0 is the positive 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 the single 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] Among them, Ω ≥ 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, and 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 the linear programming construct a mixed integer linear programming form and express it as:

[0081]

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

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

[0084]

[0085] The mixed integer linear programming form of the thruster is solved and the optimal state quantity a is obtained respectively. 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 into the formula, and obtain the thrust value of each propeller of the robotic arm as u thr =a 1_optimal ξ1+a 2_optimal ξ2+ξ0.

[0087] Furthermore, the step S7 comprises:

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

[0089]

[0090] Where m = m0 + m p is the total mass of the robotic arm, m0 is the mass of the robotic arm when it is not grabbing a 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 body when the robot arm does not grab the load, and g is the gravitational acceleration;

[0091] Step S72, define the 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 robot arm 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 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, determining 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, calculating 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, establishing 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 Taylor expansion is performed near , and the subform of the quadratic programming form is obtained, 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, establishing a quadratic programming form for a single time step k according to the subform of the quadratic programming form, the expression is:

[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 solution result is used to calculate the load mass

[0112] Further, according to the parameter θ to be estimated at the kth time step k The solution result is used to calculate the load mass 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. 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 jitter phenomenon of the integral sliding mode; in order to reduce the influence of complex environment on the motion performance of the underwater swimming manipulator, an extended state observer is set to compensate for uncertainties such as unmodeled dynamics and external disturbances of the manipulator, and at the same time, in view of the problem of decreased control performance caused by the dead zone characteristics of the thruster, and considering the special case that 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 decrease in 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. The noise caused by water flow, marine biological activities, 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. By adjusting the data fusion range of the kinematic model and the dynamic model to increase the redundant information to improve the detection accuracy, 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 robot arm, considers the acceleration change of each position degree of freedom of the base connecting rod in the world coordinate system {W}, and the influence of the load mass on the total mass of the robot arm, 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 water flow disturbances, underwater pressure and other factors on the motion performance of the manipulator, the uncertainties such as the unmodeled dynamics and external disturbances of the manipulator are compensated. At the same time, the robustness of the sliding mode control is utilized, and the load mass information is separated from other disturbances by utilizing the output of the observer to estimate the load mass, so as to cope 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 allocation algorithm of mixed integer linear programming, and 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 manipulator; the underwater swimming manipulator 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 manipulator. In view of the problem of decreased control performance caused by the dead zone characteristics of the propeller, a thrust allocation algorithm based on mixed integer linear programming is set, and integer auxiliary decision variables yi and zi are introduced to construct a mixed integer linear programming form, and the mixed integer linear programming form is solved to obtain a thrust value that meets the thrust allocation requirements and avoids falling into the dead zone range, and then handles the special case where the output value of the dead zone boundary 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 form of quadratic programming and improve the accuracy of parameter estimation results, the load mass estimation results under continuous time steps are iteratively optimized until a preset number of iterations is reached or the convergence conditions are met, thereby improving the accuracy of 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 manipulator based on load estimation according to an embodiment of the present invention;

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

[0124] Figure 3 It 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 A schematic diagram of changes in pitch and yaw joint angles 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;

[0126] Figure 5 A comparison diagram of the position of an underwater swimming mechanical arm and an expected value according to a method for controlling an underwater swimming mechanical arm 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 robot arm according to an underwater swimming robot arm control method based on load estimation according to an embodiment of the present invention;

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

[0130] Fig. 9 It is a schematic diagram of the estimation result 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] Fig.10 It is a schematic diagram of the estimation result of the disturbance of each posture 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;

[0132] Fig.11 Thrust diagrams of 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] Fig.12 Thrust diagrams of 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] Fig.13 A load mass variation diagram of an underwater swimming manipulator control method based on load estimation according to an embodiment of the present invention;

[0135] Explanation 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 only used to explain the present invention and are not used 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 protection scope 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 drawings. This is merely 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] In addition, it should be noted that in the description of the present invention, unless otherwise clearly specified and limited, the terms "installed", "connected", and "connected" should be understood in a broad sense, for example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be a direct connection, or it can be indirectly connected through an intermediate medium, or it can be the internal communication of two components. For those skilled in the art, the specific meanings of the above terms in the present invention can be understood according to specific circumstances.

[0140] See also Figure 1 , Figure 2 , Figure 3 As shown, they are respectively an overall flow chart of the underwater swimming manipulator control method based on load estimation, a model schematic diagram of the underwater swimming manipulator, and a logic flow chart of the embodiment of the present invention. 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 robot arm according to the motion data of the robot arm;

[0142] Step S2, determining whether the degree of environmental interference meets the requirements according to the underwater noise, and determining an adjustment method when 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, respectively calculating output parameters of an integral sliding mode controller and compensation output parameters of an extended state observer based on the kinematic model, the dynamic model and the current posture;

[0145] Step S5, calculating the torque of the robot arm 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 parameter 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 the steps S3 to S8 until the robot 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 jitter 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 unmodeled dynamics and external disturbances of the manipulator. At the same time, in view of the problem of decreased control performance 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 decrease in 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.

[0151] Specifically, the step S2 includes:

[0152] Acquiring underwater noise at the 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 the 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 the 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, the 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 speed of the robot's posture change is the ratio of the resultant vector of the robot's movement to the unit time.

[0162] Specifically, the data fusion scope 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 preferred embodiment of the preset first underwater noise is 220 Hz, and the preferred embodiment of the preset 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] In implementation, the method of the present invention sets a preset underwater noise. The noise caused by water flow, marine biological activities, ship vibration, sound waves generated by other equipment such as sonar, etc. will mix with the transmitted signal, resulting in reduced signal strength and a decrease in signal-to-noise ratio, which in turn leads to a decrease in the accuracy of the detected data. The noise caused by the stirring of the robotic arm is suppressed by reducing the posture change speed of the robotic arm, and the data fusion range of the kinematic model and the dynamic model is adjusted to improve the detection accuracy by increasing redundant information, thereby increasing the observation accuracy of the expanded state observer.

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

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

[0169] The position and posture of each link {Bi} of the robot arm in the world coordinate system {W} are calculated according to the state vector and the velocity vector. The position and posture are expressed as:

[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 matrix of zeros.

[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] Among them, 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 connecting rod 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 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 as follows:

[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 calculating the output parameter 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 surface, and c2 is the positive definite diagonal matrix of the integral gain of the integrated sliding surface;

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

[0190] The output parameter of the integral sliding mode controller is determined according to 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 , it is determined that the robotic arm reaches the sliding surface.

[0196] Specifically, the 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 mechanical 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, determining 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 moment τ to obtain a general solution for the thrust value of the propeller;

[0207] Among them, 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 water flow disturbances, underwater pressure and other factors on the motion performance of the manipulator, the uncertainties such as the unmodeled dynamics and external disturbances of the manipulator are compensated. At the same time, the robustness of the sliding mode control is utilized, and the load mass information is separated from other disturbances by utilizing the output of the observer to estimate the load mass, so as to cope with the dead zone characteristics of the thruster, thereby improving the adaptability and stability of the manipulator in complex underwater environments.

[0210] Specifically, the 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 an auxiliary variable u into the objective function i (i=1,2,…,n), and add constraint u i ≥|u thri |;

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

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

[0217] Step S65, when the thrust value range is 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 the single 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] Among them, Ω ≥ 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, and 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 the linear programming construct a mixed integer linear programming form and express it as:

[0228]

[0229]

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

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

[0232]

[0233] Solve the integer linear programming form of the thruster and obtain the state quantity a 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 into the formula, and obtain the thrust value of each propeller of the robotic arm as u thr =a 1_optimal ξ1+a 2_optimal ξ2+ξ0.

[0235] In implementation, the method of the present invention introduces a thrust allocation 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 manipulator. The underwater mobile manipulator mainly relies on the propeller to adjust its own posture when performing work tasks. However, 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 mobile manipulator. In view of the problem of decreased control performance caused by the dead zone characteristics of the propeller, a thrust allocation algorithm based on mixed integer linear programming is set, and integer auxiliary decision variables yi and zi are introduced to construct a mixed integer linear programming form, and the mixed integer linear programming form is solved to obtain the thrust value that meets the thrust allocation requirements and avoids falling into the dead zone range, and then handles the special case where the output value of the dead zone boundary is non-zero, thereby enhancing the practicality and adaptability of the algorithm.

[0236] Specifically, the step S7 includes:

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

[0238]

[0239] Where m = m0 + m p is the total mass of the robotic arm, m0 is the mass of the robotic arm when it is not grabbing a 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 body when the robot arm does not grab the load, and g is the gravitational acceleration;

[0240] Step S72, define the 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 robot arm 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 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, determining 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, calculating 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, establishing 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 kTaylor expansion is performed near , and the subform of the quadratic programming form is obtained, 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, establishing a quadratic programming form for a single time step k according to the subform of the quadratic programming form, the expression is:

[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 solution result is used to calculate the load mass

[0260] Specifically, according to the parameter θ to be estimated at the kth time step k The solution result is used to calculate the load mass 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. 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 such as mass distribution and inertia parameters of the underwater swimming robotic arm 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 form of quadratic programming and improve the accuracy of parameter estimation results, the load mass estimation results under continuous time steps are iteratively optimized until a preset number of iterations is reached or the convergence conditions are met, thereby improving the accuracy of continuous and precise estimation of the load mass and improving the motion stability and load adaptability of the robotic arm.

[0265] Embodiment 1, as Figure 4 , Figure 5 , Figure 6 , Figure 7 , Figure 8 , Fig. 9 , Fig.10 , Fig.11 , Fig.12 as well as Fig.13 As shown, they are respectively a schematic diagram of the pitch and yaw joint angle changes of the underwater swimming manipulator arm of the underwater swimming manipulator arm 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 graph of the underwater swimming manipulator arm position error, a graph of the error curve 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 robot arm is to start moving at the position of the world coordinate system (-2m, -2m, -2m) at t = 0s, move to the origin of the world coordinate system at t = 40s, and remain calm at this position until t = 60s. During the movement, the base posture is always maintained at (0rad, 0rad, 0rad). The underwater swimming robot arm 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 into 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 perform operations according to the thrust values.

[0284] Specifically, the pitch and yaw joint angles of the manipulator arm are analyzed during the movement process of the manipulator arm performing actions according to the thrust value, 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 disturbance of each position degree of freedom by the extended state observer are calculated, the estimation results of the disturbance of each posture degree of freedom by the extended state observer 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. Fig. 9 and Fig.10 The extended state observer is shown to accurately estimate the uncertainties; Fig.11 and Fig.12 As shown, the thrust of each thruster avoids the dead zone and no obvious buffeting occurs; Fig.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 operating stability of the robotic arm.

[0287] So far, the technical solutions of the present invention have been described in conjunction with the preferred embodiments shown in the accompanying drawings. However, it is easy for those skilled in the art to understand that the protection scope of the present invention is obviously not limited to these specific embodiments. Without departing from the principle of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the protection scope of the present invention.

Claims

1. A method for controlling an underwater mobile robot arm based on load estimation, characterized in that: include: Step S1, establishing a kinematic model and a dynamic model of the robot arm according to the motion data of the robot arm; Step S2, determining whether the degree of environmental interference meets the requirements according to the underwater noise, and determining an adjustment method when 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, respectively calculating output parameters of an integral sliding mode controller and compensation output parameters of an extended state observer based on the kinematic model, the dynamic model and the current posture; Step S5, calculating the torque of the robot arm 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 parameter 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 the steps S3 to S8 until the robot 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 comprises: Acquiring underwater noise at the 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 requirement; 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 the 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 the 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 robot arm in the world coordinate system {W}, define the velocity vector of the robot arm in the base coordinate system {B1}, and define the motion rotation of each connecting rod of the robot arm; The position and posture of each link {Bi} of the robot arm in the world coordinate system {W} are calculated according to the state vector and the velocity vector. The position and posture are expressed as: 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 matrix of zeros.

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; Among them, 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 comprises: 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 as follows: in, is the state observation value vector; ρ1, ρ2, ρ3 are the gains of the extended state observer; 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 comprises: 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}; 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 surface, and c2 is the positive definite diagonal matrix of the integral gain of the integrated sliding surface; The derivative of the integral sliding surface is obtained: Right now The output parameter of the integral sliding mode controller is determined according to 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 , 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 mechanical 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, determining 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 moment τ to obtain a general solution for the thrust value of the propeller; Among them, 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 an auxiliary variable u into the objective function i (i=1,2,…,n), and add constraint u i ≥|u thri |; Step S63, determining the final form of the objective function according to the constraint conditions: Step S64, setting the dead zone constraint u thri ≤d - oru thri ≥d + , where d + >0 is the positive dead zone of the thruster, d - <0 is the reverse dead zone of the propeller; Step S65, when the thrust value range is 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 the single thrust value i and z i , in, And y i +z i =1; Step S67, according to the integer auxiliary decision variable y i and z i Identify linear constraints: Among them, Ω ≥ 10 6 ; Step S68, introduce the standard form of linear programming, expressed as: s.t.x≤x c or x≥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, and 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 the linear programming construct a mixed integer linear programming form and express it as: Among them, x Z For x c Integer decision variables in ; Step S610, constructing a mixed integer linear programming form of the propeller according to the mixed integer linear programming form, the expression is: The mixed integer linear programming form of the thruster is solved and the optimal state quantity a is obtained respectively. 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 into the formula, and obtain the thrust value of each propeller of the robotic arm as u 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 comprises: Step S71, establishing load parameters of the robotic arm according to the dynamic model; Where m = m0 + m p is the total mass of the robotic arm, m0 is the mass of the robotic arm when it is not grabbing a 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 body when the robot arm does not grab the load, and g is the gravitational acceleration; Step S72, define the 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 robot arm into a parameter estimation equation of b=Aθ; Step S73, establish the inequality constraint {a xk , a yk , a zk }, expressed as, Among them, [x g0 y g0 z g0 ] is the coordinate of the center of mass position 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, determining 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, calculating 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, establishing 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 Taylor expansion is performed near , and the subform of the quadratic programming form is obtained, 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, establishing a quadratic programming form for a single time step k according to the subform of the quadratic programming form, the expression is: 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 solution result is used to calculate the load mass 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 solution result is used to calculate the load mass 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. The first dimension according to Get the load mass

Images