Underwater swimming manipulator stabilization control and thrust distribution method in complex environment

Abstract

The present application relates to the field of mechanical arm stabilization control, and particularly relates to a method for underwater swimming mechanical arm stabilization control and thrust distribution under complex environment, which establishes a kinematic model and a dynamic model of the underwater swimming mechanical arm, and decouples the kinematic model and the dynamic model into a double-integral linearization model; an inner-layer generalized super-spiral adaptive sliding mode controller is designed to compensate for errors caused by model linearization and external disturbances, and a model predictive controller is determined based on the double-integral linearization dynamic model; based on the output of the controller, a sequential quadratic programming thrust distribution algorithm with a penalty function is designed to calculate the thrust of each propeller of the underwater swimming mechanical arm. The present application can effectively deal with the control challenges of the underwater swimming mechanical arm in aspects such as hovering stabilization and propeller dead zone, is suitable for many systems with strong coupling and nonlinear characteristics and motion control systems with dead zones or physical constraints, and improves the precise control of the mechanical arm in complex environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the field of mechanical arm stabilization control, in particular to a method for stabilization control and thrust distribution of an underwater swimming mechanical arm in a complex environment. BACKGROUND

[0002] With the development and utilization of marine resources in China, underwater robots and other carriers for marine surveying, detection and other operations have been increasingly widely used. As a new type of underwater robot, the underwater swimming mechanical arm adopts a multi-joint chain bionic snake design and is equipped with multiple thrusters, has the characteristics of high degree of freedom, strong operability, flexibility and controllability, can adapt to changing underwater environments, and is good at performing tasks in small spaces. The hover stabilization control of the underwater swimming mechanical arm is the basis for it to realize autonomous operation tasks. Through accurate hover control, the robot can remain stable at a specific location and perform long-term observation or operation tasks. Research on the hover stabilization control technology of the underwater swimming mechanical arm helps to improve its operation accuracy and stability in complex marine environments, which is crucial for fine underwater operations.

[0003] For example, Chinese Patent Publication No. CN114714359A discloses a saturation parameterized control method for a mechanical arm system, which includes establishing a dynamic model of the mechanical arm system, designing a controller for the mechanical arm system, linearizing the actuator saturation, converting the system model, introducing a parameterized matrix, and solving the system gain matrix. The invention is based on a parameterized method, taking into account the influence of actuator saturation, and designs a controller suitable for the mechanical arm system. By eliminating the dynamic characteristics of the open-loop system, a system with desired closed-loop characteristics is obtained, avoiding the destruction of the system's full drive characteristics and achieving the stabilization control of the mechanical arm system.

[0004] For example, Chinese Patent Publication No. CN118456437A discloses a control method and device for a flexible mechanical arm, electronic equipment and storage medium. The method constructs a non-singular integral terminal sliding mode surface according to a positive definite diagonal matrix and a trajectory tracking error, which has high robustness, high precision and fast transient response characteristics. Based on the dynamics model of the flexible mechanical arm, combined with the non-singular integral terminal sliding mode surface, the virtual control law is constructed layer by layer using the backstepping method layer-by-layer design strategy to obtain the input control torque of the flexible mechanical arm. The input control torque is used as the control instruction of the flexible mechanical arm to adjust the flexible mechanical arm to achieve tracking control of the desired trajectory, ensuring that the sliding mode controller converges to 0 in a limited time, and the convergence time can be controlled to a certain extent by parameters, meeting the rapid demand for complex body stabilization after capture, and maintaining a small steady-state error and torque chattering.

[0005] However, the prior art still has the following problems,

[0006] When the underwater mobile manipulator relies on the thruster to realize the change of its own pose during the execution of the work task, the existence of the thruster operation dead zone may have an adverse effect on the work task of the underwater mobile manipulator, so that the motion control of the underwater robot becomes complex, and the stability and control accuracy are affected. SUMMARY

[0007] To this end, the present application provides a kind of underwater mobile manipulator stabilization control and thrust distribution method in complex environment, to solve underwater mobile manipulator in the execution of the work task, when the thruster is relied on to realize the change of its own pose, the existence of the thruster operation dead zone can have an adverse effect on the work task of the underwater mobile manipulator, so that the motion control of the underwater robot becomes complex, and the stability and control accuracy are affected.

[0008] To achieve the above object, the present application provides a kind of underwater mobile manipulator stabilization control and thrust distribution method in complex environment, which includes:

[0009] The kinematics and dynamics model of the underwater mobile manipulator is established, and the kinematics and dynamics model is decoupled into double integral linearization dynamics model;

[0010] An inner layer generalized superhelix adaptive sliding mode controller is designed to compensate for the error caused by model linearization and external disturbance;

[0011] Based on the double integral linearization dynamics model, a model predictive controller is determined, and the controller constraint is adjusted in real time according to the output of the inner layer sliding mode controller and the change of the joint angle of the underwater mobile manipulator, so that the underwater mobile manipulator can obtain the optimal control sequence that meets the time-varying constraint condition;

[0012] Based on the output of the model predictive controller, a sequence quadratic programming thrust distribution algorithm with penalty function is designed, and the thrust of each thruster of the underwater mobile manipulator is calculated to realize the stabilization control of the underwater mobile manipulator in complex underwater environment.

[0013] Further, the process of establishing the kinematics and dynamics model of the underwater mobile manipulator includes,

[0014] The world coordinate system and the link coordinate system of the underwater mobile manipulator are established based on the right-hand rule;

[0015] The pose of the link of the underwater mobile manipulator in the world coordinate system is calculated based on the rotation matrix of the link coordinate system relative to the world coordinate system;

[0016] The kinematics and dynamics model of the underwater mobile manipulator is determined based on the pose of the link;

[0017] The pose includes a position and an attitude of the link in a world coordinate system.

[0018] Further, the process of calculating the pose of the link of the underwater swimming manipulator in the world coordinate system comprises,

[0019] determining a rotation matrix and a position vector relative to the world coordinate system based on the link coordinate system;

[0020] determining a combined matrix of the rotation matrix and the position vector as the attitude of the link;

[0021] determining the pose according to the attitude and a combination of rotation and translation along the screw axis.

[0022] Further, the process of decoupling the kinematics and dynamics model into a double-integral linearized dynamics model comprises,

[0023] determining a state variable;

[0024] adjusting the kinematics and dynamics model of the underwater swimming manipulator into a state space expression based on the state variable;

[0025] designing an inverse dynamics nonlinear compensation control law containing control instructions based on the kinematics and dynamics model;

[0026] determining a combination of the state space expression and the inverse dynamics nonlinear compensation control law as a double-integral linearized dynamics model.

[0027] Further, the state variable comprises a position vector and a velocity vector.

[0028] Further, the process of designing the inner-layer generalized super-spiral adaptive sliding mode controller comprises,

[0029] determining a generalized super-spiral sliding mode controller with an adaptive gain;

[0030] adjusting the control gain adaptively based on an update law to determine the comprehensive control performance of the controller;

[0031] determining the generalized super-spiral sliding mode controller with the comprehensive control performance as the inner-layer generalized super-spiral adaptive sliding mode controller.

[0032] Further, the process of determining a model predictive controller based on the double-integral linearized dynamics model comprises,

[0033] determining a state variable, an input variable and an output variable of the underwater swimming manipulator;

[0034] constructing a state transition equation of the underwater swimming manipulator based on the state variable, the input variable and the output variable;

[0035] determine a model predictive controller based on the state transition equation.

[0036] Further, the model predictive controller generates pseudo control instructions, and the constraint conditions need to be updated in real time.

[0037] Further, the constraint conditions include a system generalized inertia matrix, a limit value of a generalized force calculated by a configuration and mechanical parameters of a thruster of the underwater mobile manipulator, a limit value of a moment calculated by the configuration and mechanical parameters of the thruster of the underwater mobile manipulator, an output of a sliding mode controller, and an output of the model predictive controller.

[0038] Further, the process of designing the sequence quadratic programming thrust allocation algorithm with a penalty function comprises,

[0039] calculating a thrust allocation matrix based on a thruster configuration of the underwater mobile manipulator;

[0040] performing Taylor expansion on an objective function of the original nonlinear constraint problem near an iteration point to obtain a description of a quadratic programming sub-problem;

[0041] writing a discontinuous inequality constraint in the description of the quadratic programming sub-problem in the form of a penalty function;

[0042] determining a description of a quadratic programming problem based on the thrust allocation matrix and the description of the quadratic programming sub-problem.

[0043] Compared with the prior art, the underwater mobile manipulator kinematics and dynamics model is established, the kinematics and dynamics model is decoupled into a double-integral linearized dynamics model, an inner-layer generalized super-spiral adaptive sliding mode controller is designed to compensate for model errors and external disturbances, a model predictive controller is determined based on the double-integral linearized dynamics model to ensure that the controlled system can be adjusted in real time according to the sliding mode controller output and the underwater mobile manipulator joint angle change, a sequence quadratic programming thrust allocation algorithm with a penalty function is designed based on the output of the model predictive controller to calculate the thrust of each thruster of the underwater mobile manipulator, so as to realize the stabilization control of the underwater mobile manipulator in a complex underwater environment. The control scheme proposed in the application can effectively cope with the control challenges of the underwater mobile manipulator in aspects of hovering stabilization and thruster dead zone, is suitable for many systems with strong coupling nonlinear characteristics and motion control systems with dead zones or physical constraints, to a certain extent, eliminates the chattering phenomenon of the controller, reduces the energy consumption of the underwater mobile manipulator, and improves the precise control ability of the manipulator in a complex environment.

[0044] Especially, the application decouples the dynamic model of the underwater swimming mechanical arm into a double-integral linearization model, in which errors are easy to accumulate and propagate between degrees of freedom, resulting in a decrease in control accuracy, and through decoupling, errors can be effectively isolated and reduced, thereby improving control accuracy, and based on this, the application decouples the dynamic model of the underwater swimming mechanical arm into a double-integral linearization model, providing a theoretical basis for subsequent controller design, and the decoupled model eliminates or weakens the coupling effect between degrees of freedom, allowing independent control of each degree of freedom, avoiding adverse effects on other degrees of freedom when controlling one degree of freedom, improving the stability and performance of the control system, and improving the precise control of the mechanical arm in complex environments.

[0045] Especially, the application designs an inner-layer generalized super-spiral adaptive sliding mode controller to compensate for the effects of model uncertainty and complex environmental disturbances due to unmodeled dynamics and linearization, and in actual stabilization control of the mechanical arm, most controller designs are directly based on linearization models, without considering the effects of model uncertainty and environmental factors, resulting in deviations in the calculation results of the controller and instability of the mechanical arm, and based on this, the application designs an inner-layer generalized super-spiral adaptive sliding mode controller to compensate for errors caused by inaccurate models and external disturbances, making the mechanical arm more stable and improving the precise control of the mechanical arm in complex environments.

[0046] Especially, the application determines a model predictive controller based on the double-integral linearization model to enable the upper and lower bounds of the constraints in the model predictive controller to be adjusted in real time according to the output of the pre-model predictive controller and the joint angle changes of the underwater swimming mechanical arm, and in actual situations, the mechanical arm may be disturbed by external interference when operating, deviating from the predetermined target and failing to complete the predetermined work task, and based on this, the application determines the model predictive controller, receives the state information of the underwater swimming mechanical arm and the output of the sliding mode controller in real time, and updates the boundaries of the output constraints, ensuring that the mechanical arm realizes optimal control under the conditions of satisfying the state and real-time input constraints, and improving the precise control of the mechanical arm in complex environments.

[0047] In particular, the application designs a thrust allocation algorithm of sequence quadratic programming with a penalty function to avoid the output of the thruster in the dead zone or beyond the mechanical limit. In actual situations, if the generalized force calculated by the controller is not effectively processed and allocated on the thruster, some thrusters may enter the dead zone or exceed their physical limits, so that the control effect of the manipulator is poor, the preset task cannot be completed, and even the work is stopped. Based on this, the application designs a thrust allocation algorithm of sequence quadratic programming, and reconstructs the quadratic programming problem. The inequality constraint discontinuity problem caused by the thruster dead zone is integrated into the objective function in the form of a penalty function, thereby reducing the energy consumption of the underwater mobile manipulator, keeping the thruster thrust value at the optimal solution at all times, and improving the precise control of the manipulator in complex environments. BRIEF DESCRIPTION OF DRAWINGS

[0048] Figure 1 A step schematic diagram of the underwater mobile manipulator stabilization control and thrust allocation method in the complex environment of the application embodiment;

[0049] Figure 2 A model schematic diagram of the underwater mobile manipulator of the application embodiment;

[0050] Figure 3 An equivalent model schematic diagram of the application embodiment;

[0051] Figure 4 A schematic diagram of the angle change and disturbance of the underwater mobile manipulator stabilization process of the application embodiment;

[0052] Figure 5 A schematic diagram of the position angle change and error of the underwater mobile manipulator in the whole simulation process of the application embodiment;

[0053] Figure 6 A schematic diagram of the control input generated by the model predictive controller and the upper and lower bound change of the constraint;

[0054] Figure 7 A schematic diagram of the thrust of the thruster of the application embodiment;

[0055] In the figure, tail link 1, tail cabin 101, thruster cabin 102, flexible joint 103, thruster 104, thruster 105, thruster 106, thruster 107, middle link 2, main control cabin 201, thruster cabin 202, joint cabin 203, thruster 204, thruster 205, thruster 206, thruster 207, head link 3, head cabin 301. DETAILED DESCRIPTION

[0056] In order to make the objects and advantages of the present application more clear, the present application 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 application and not to limit the present application.

[0057] The preferred embodiments of the present application are described below in conjunction with the accompanying drawings. Those skilled in the art should understand that these embodiments are only used to explain the technical principles of the present application and not to limit the protection scope of the present application.

[0058] Please refer to Figure 1 , Figure 1 The steps of the underwater swimming mechanical arm stabilization control and thrust distribution method in a complex environment of an embodiment of the present application are shown in the schematic diagram. The underwater swimming mechanical arm stabilization control and thrust distribution method in a complex environment of the present application comprises:

[0059] A kinematics and dynamics model of the underwater swimming mechanical arm is established, and the kinematics and dynamics model is decoupled into a double-integral linearized dynamics model;

[0060] An inner-layer generalized super-spiral adaptive sliding mode controller is designed to compensate for the errors caused by model linearization and external disturbances;

[0061] A model predictive controller is determined based on the double-integral linearized dynamics model, and the controller constraints are adjusted in real time according to the output of the inner-layer sliding mode controller and the underwater swimming mechanical arm joint angle changes, so that the underwater swimming mechanical arm can obtain an optimal control sequence that meets the time-varying constraint conditions;

[0062] Based on the output of the model predictive controller, a sequence quadratic programming thrust distribution algorithm with a penalty function is designed to calculate the thrust of each propeller of the underwater swimming mechanical arm, so as to realize the stabilization control of the underwater swimming mechanical arm in a complex underwater environment.

[0063] Specifically, the links of the underwater swimming mechanical arm are numbered as link 1, link 2, …, link n from the tail to the head, wherein link i and link i+1 are connected through joint i, and the rotation direction of the i-th joint is defined with reference to the coordinate system of the i+1-th link.

[0064] Specifically, the process of establishing the kinematics and dynamics model of the underwater swimming mechanical arm comprises,

[0065] The world coordinate system and the link coordinate system of the underwater swimming mechanical arm are established based on the right-hand rule;

[0066] The pose of the link of the underwater swimming mechanical arm in the world coordinate system is calculated based on the rotation matrix of the link coordinate system relative to the world coordinate system;

[0067] determine a kinematics and dynamics model of the underwater swimming manipulator based on the pose of the link;

[0068] The pose includes a position and an attitude of the link in a world coordinate system.

[0069] Specifically, a coordinate system is established based on the right-hand rule, and the establishment of the coordinate system can be performed by a person skilled in the art according to actual conditions, which will not be described herein.

[0070] Specifically, the kinematics and dynamics model of the underwater swimming manipulator is represented by formula (1),

[0071]

[0072] In formula (1), M(ξ) represents a system generalized inertia matrix, abbreviated as M, N represents a vector of model uncertainties and external disturbances and other factors, τ represents a generalized force and torque of the manipulator, ζ represents a velocity vector of the underwater swimming manipulator in a base coordinate system, and ξ is represented by formula (2).

[0073] ξ=[x y zφθψq T ] T (2)

[0074] In formula (2), x represents a position vector of the underwater swimming manipulator in an x direction of a world coordinate system, y represents a position vector of the underwater swimming manipulator in a y direction of the world coordinate system, z represents a position vector of the underwater swimming manipulator in a z direction of the world coordinate system, φ represents an attitude vector of roll, θ represents an attitude vector of pitch, ψ represents an attitude vector of yaw, and q represents a joint angle vector.

[0075] Specifically, the system generalized inertia matrix includes mapping of inertias of each link and additional mass forces to the base coordinate system.

[0076] Specifically, the process of calculating the pose of the link of the underwater swimming manipulator in the world coordinate system includes,

[0077] determining a rotation matrix and a position vector relative to the world coordinate system based on the link coordinate system;

[0078] determining a combined matrix of the rotation matrix and the position vector as the attitude of the link;

[0079] determining the pose according to the attitude and a combination of two motion modes of rotation along an axial vector and translation.

[0080] Specifically, the attitude of the link is represented by formula (3),

[0081]

[0082] In Equation (3), denotes a rotation matrix of the link coordinate system with respect to the world coordinate system, denotes a position vector,

[0083] Specifically, an infinitesimal representation method in screw motion is adopted, and the instantaneous linear velocity and angular velocity of the link are described by a screw, which is regarded as a generalization of a skew-symmetric matrix The screw is expressed by Equation (4),

[0084]

[0085] In Equation (4), denotes a linear velocity vector of the joint i, denotes an angular velocity vector of the joint i, denotes a skew-symmetric matrix of The operator ^ denotes a mapping of and se(3) denotes a Lie algebra of a special Euclidean group.

[0086] Specifically, the pose of each link is expressed by Equation (5),

[0087] H i+1 = H i A i (θ i ) (5)

[0088] In Equation (5), θ i denotes an angle of the i-th joint, and A i (θ i ) denotes a combination of rotation and translation of the i-th link along the screw axis.

[0089] Specifically, the combination is expressed by Equations (6)-(8),

[0090]

[0091] In Equations (6)-(8), I denotes an inertia matrix, A i,j (θ) denotes a pose transformation matrix between the i-th link and the j-th link, A j-1 (θ j-1 ) denotes a homogeneous transformation matrix of the j-1-th link, θ j-1 denotes an angle of the j-1-th joint angle, and A j,i denotes a pose transformation matrix between the j-th link and the i-th link.

[0092] Specifically, the axis of the motion screw is the screw axis, and the motion screw is represented by formula (9),

[0093]

[0094] In formula (9), represents the linear velocity of the link in the link coordinate system, represents the angular velocity of the link in the link coordinate system.

[0095] Specifically, the Jacobian matrix J i The relationship between the generalized velocity in the base coordinate system of the underwater swimming manipulator and the motion screw of each link in the link coordinate system is described by formula (10),

[0096]

[0097] In formula (10), represents the motion screw of the base, represents the joint angular velocity.

[0098] Specifically, the Jacobian matrix J i is represented by formula (11) and formula (12),

[0099] J1=[I 6×6 0 6×n ] (11)

[0100]

[0101] In formula (11) and formula (12), Ad -1 (A i ) represents the adjoint operator of link i, Ad -1 (A 1,i ), Ad -1 (A 2,i ) is a specific representation of Ad- 1 (Aj,i), and Ad- 1 (Aj,i) represents the coadjoint operator, represents the screw coordinates of joint i, and the adjoint operator Ad -1 (A i ) and the coadjoint operator Ad -1 (A j,i ) are represented by formula (13),

[0102]

[0103] In formula (13), represents the rotation matrix of link i relative to the world coordinate system, represents the position vector of link i relative to the world coordinate system, represents a rotation matrix of link i with respect to link j coordinate system, represents a position vector of link i with respect to link j coordinate system.

[0104] Specifically, the process of decoupling the kinematics and dynamics model into a double-integral linearized dynamics model comprises,

[0105] determining a state variable;

[0106] adjusting the underwater swimming robotic arm kinematics and dynamics model into a state space expression based on the state variable;

[0107] designing an inverse dynamics nonlinear compensation control law containing control instructions based on the kinematics and dynamics model;

[0108] determining a combination of the state space expression and the inverse dynamics nonlinear compensation control law as a double-integral linearized dynamics model.

[0109] Specifically, the state variable comprises a position vector and a velocity vector.

[0110] Specifically, the state variable is represented by formula (14),

[0111]

[0112] In formula (14), x1 represents a position vector under the base coordinate system, and x2 represents a velocity vector under the base coordinate system.

[0113] Specifically, adjusting the underwater swimming robotic arm kinematics and dynamics model into a state space expression based on the state variable is represented by formula (15),

[0114]

[0115] In formula (15), represents a state space expression of a position vector under the base coordinate system, represents a state space expression of a velocity vector under the base coordinate system.

[0116] It can be understood that designing an inverse dynamics nonlinear compensation control law containing control instructions based on the kinematics and dynamics model is represented by formula (16),

[0117] τ = M (u mpc + u GSTA ) (16)

[0118] In formula (16), u mpc represents an output value of a model predictive controller, and u GSTAAn output value of the sliding mode controller.

[0119] Further, the inverse dynamics nonlinear compensation control law is substituted into the state space expression to obtain a double-integral linearized dynamics model.

[0120] Specifically, the application decouples the dynamics model of the underwater swimming mechanical arm into a double-integral linearized model, in which errors are easy to accumulate and propagate between degrees of freedom, resulting in a decrease in control accuracy, and through decoupling, errors can be effectively isolated and reduced, thereby improving control accuracy, and based on this, the application decouples the dynamics model of the underwater swimming mechanical arm into a double-integral linearized model, providing a theoretical basis for subsequent controller design, and the decoupled model eliminates or weakens the coupling effect between degrees of freedom, allowing independent control of each degree of freedom, avoiding adverse effects on other degrees of freedom when controlling one degree of freedom, improving the stability and performance of the control system, and improving the precise control of the mechanical arm in complex environments.

[0121] Specifically, the process of designing the inner-layer generalized super-spiral adaptive sliding mode controller includes,

[0122] determining a generalized super-spiral sliding mode controller with adaptive gain;

[0123] adaptively adjusting the control gain based on the update law to determine the comprehensive control performance of the controller;

[0124] determining the generalized super-spiral sliding mode controller with comprehensive control performance as the inner-layer generalized super-spiral adaptive sliding mode controller.

[0125] Specifically, the generalized super-spiral sliding mode controller with adaptive gain is represented by formula (17),

[0126]

[0127] In formula (17), sigma represents the state error of the mechanical arm, k1 represents the first gain of the controller, k2 represents the second gain of the controller, beta represents the third gain of the controller, and eta represents a continuous but non-smooth function.

[0128] Specifically, when the system moves on the sliding surface, sigma is approximately 0, weakening the step characteristic of the sign function, and eta plays a role in suppressing disturbances, allowing the system to maintain strong robustness and converge in a finite time in the presence of disturbances, the sign function in eta is hidden in its differential term, ensuring the continuity of the output and weakening the chattering of the system, and the value of k1 determines the comprehensive control performance of the system, such as the convergence time of the system in an ideal state, and the value of k2 determines the robustness and disturbance rejection ability of the mechanical arm.

[0129] Specifically, the update law is represented by formula (18),

[0130]

[0131] In formula (18), epsilon represents a first normal number, lambda represents a second normal number, gamma 1 represents a third normal number, and omega 1 represents a fourth normal number.

[0132] Specifically, the inner-layer generalized super-spiral adaptive sliding mode controller is designed, the influence of model uncertainty caused by unmodeled dynamics and linearization and complex environmental disturbance is compensated, when the mechanical arm is actually stabilized and controlled, the controller is directly designed based on the linearized model in most cases, the influence of model uncertainty and environmental factors is not considered, the calculation result of the controller is deviated, the mechanical arm is unstable, based on this, the inner-layer generalized super-spiral adaptive sliding mode controller is designed, the error caused by the model and the external disturbance is compensated, so that the mechanical arm is more stable, and the precise control of the mechanical arm in the complex environment is improved.

[0133] Specifically, the process of determining the model predictive controller based on the double-integral linearized dynamics model comprises,

[0134] The state quantity, input quantity and output quantity of the underwater swimming mechanical arm are determined.

[0135] The state transition equation of the underwater swimming mechanical arm is constructed based on the state quantity, input quantity and output quantity.

[0136] The model predictive controller is determined based on the state transition equation.

[0137] Specifically, the state quantity of the mechanical arm is represented by formula (19),

[0138] x(k)=[x1 x2 x3 x4 x5 x6] T

[0139] In formula (19), x1=x, x2=y, x3=z, x4=phi, x5=theta, and x6=psi.

[0140] The input quantity of the mechanical arm is represented by formula (20),

[0141] u(k)=[u1 u2 u3 u4 u5 u6] T

[0142] In formula (20), u1=x, u2=y, u3=z, u4=phi, u5=theta, and u6=psi.

[0143] The output quantity of the mechanical arm is represented by formula (21), ​​

[0144] y(k) = [y1 y2 y3 y4 y5 y6] T (21)

[0145] In formula (21), y1 = x, y2 = y, y3 = z, y4 = φ, y5 = θ, and y6 = ψ.

[0146] Specifically, the state transition equation is represented by formula (22),

[0147]

[0148] The model predictive controller is represented by formula (23),

[0149]

[0150] The definition of each vector generated in the prediction process is represented by formula (24),

[0151]

[0152] In formulas (22)-(24), f(x(k), u(k)) represents the mapping relationship between the input quantity u(k) and the state quantity x(k), x(k+n|k) represents the n-th state quantity in the prediction time domain at the k time, y(k+n|k) represents the n-th output quantity in the prediction time domain at the k time, n is a positive integer, k represents the time step, N p represents the prediction time domain, N c represents the control time domain.

[0153] Specifically, the linear model predictive control algorithm is converted into a planning problem in a finite time domain, and the optimal control sequence can be calculated, as shown in formula (25):

[0154]

[0155] In formula (25), J M represents the linear prediction output equation cost function, τ min represents the lower limit of the generalized force of the underwater swimming manipulator, τ max represents the upper limit of the generalized force.

[0156] By solving formula (25), the optimal control sequence in the control time domain can be obtained, as shown in formula (26),

[0157] U * (k) = [u * (k) T u * (k+1|k) T …u * (k+N c-1|k) T ] T (26)

[0158] In formula (26), u * (k) T represents the control input at time k.

[0159] wherein the cost function is represented by formula (27),

[0160] J M =(Y-Y d ) T Q(Y-Y d )+U T RU (27)

[0161] In formula (27), Y d represents the set trajectory in the prediction horizon, Q represents the first positive diagonal weight matrix, and R represents the second positive diagonal weight matrix.

[0162] Specifically, the model predictive controller is designed based on the double-integral linearization model, so that the upper and lower bounds of the constraints in the prediction output equation can be adjusted in real time according to the output of the sliding mode controller and the change of the joint angle of the underwater swimming manipulator. In actual situations, the manipulator may deviate from the preset target and fail to complete the preset task when it is in operation, and based on this, the model predictive controller is designed to receive the state information of the underwater swimming manipulator and the output of the sliding mode controller in real time, and update the boundary of the output constraint, so as to ensure that the manipulator realizes optimal control under the conditions of meeting the state and real-time input constraints, and improve the accurate control of the manipulator in complex environments.

[0163] Specifically, the model predictive controller generates a pseudo control instruction, and the constraint condition needs to be updated in real time.

[0164] Specifically, the constraint condition includes the generalized inertia matrix of the system, the limit value of the generalized force calculated by the configuration and mechanical parameters of the underwater robot thruster, the limit value of the torque calculated by the configuration and mechanical parameters of the underwater robot thruster, the output of the sliding mode controller and the output of the model predictive controller.

[0165] Specifically, the constraint condition is represented by formula (28),

[0166] M -1 (τ min -Mu GSTA )<u mpc <M -1 (τ max -Mu GSTA ) (28)

[0167] It can be understood that the value of M is updated in real time with the change of the joint angle of the underwater swimming manipulator.

[0168] Specifically, the process of designing a sequential quadratic programming thrust allocation algorithm with a penalty function includes,

[0169] Based on the configuration of the thrusters of the underwater swimming manipulator, a thrust allocation matrix is calculated;

[0170] The objective function of the original nonlinear constraint problem is Taylor expanded near the iteration point to obtain a description of the quadratic programming sub-problem;

[0171] The discontinuous inequality constraint is written into the description of the quadratic programming sub-problem in the form of a penalty function;

[0172] Based on the thrust allocation matrix and the description of the quadratic programming sub-problem, a description of the quadratic programming problem is determined.

[0173] Specifically, the control input of the actuator is represented by formula (29),

[0174]

[0175] In formula (29), u thr represents the control input of the m thrusters, u q represents the control input of the n joint motors,

[0176] The thrust allocation of the underwater swimming manipulator is represented by formula (30),

[0177] τ=Tu (30)

[0178] In formula (30), τ represents the generalized force and torque, T represents the thrust allocation matrix,

[0179] The thrust allocation matrix is represented by formula (31),

[0180]

[0181] In formula (31), represents the Jacobian matrix of the first group of thruster modules on the connecting rod, represents the Jacobian matrix of the second group of thruster modules on the connecting rod, Β joint represented by formulas (32)-(34),

[0182]

[0183] In formulas (32)-(34), γ βi,j represents the position of the jth propeller on the ith link in the link coordinate system, β βi,j represents the direction of the jth propeller on the ith link in the link coordinate system,

[0184] Specifically, a general nonlinear programming problem is represented by formula (35),

[0185]

[0186] In formula (35), f(x) represents the objective function, g i (x) represents the inequality constraint, and h j (x) represents the equality constraint.

[0187] Specifically, the objective function of the original nonlinear constraint problem is Taylor expanded near the iteration point x k , which can be approximated as a linear function, and a quadratic programming sub-problem is obtained, which is represented by formula (36),

[0188]

[0189] In formula (36), Δx represents the step vector, H k represents the Hessian matrix approximation of the objective function, and xk represents the iteration point.

[0190] In implementation, the discontinuous inequality constraint is written into the quadratic programming sub-problem in the form of a penalty function, and the description formula of the quadratic programming problem is obtained in combination with the thrust distribution matrix and the description formula of the quadratic programming sub-problem, which is represented by formula (37),

[0191]

[0192] In formula (37), u i represents the thrust of each propeller, u min represents the lower limit of the propeller thrust, u max represents the upper limit of the propeller thrust, C represents the penalty coefficient, C = 1 × 10 6 , and u represents the initial value of the quadratic programming problem,

[0193] Specifically, the application designs a thrust allocation algorithm of sequential quadratic programming with a penalty function to avoid the output of the thruster in the dead zone or beyond the mechanical limit, in actual situations, if the generalized force calculated by the controller is not effectively processed and allocated on the thruster, part of the thruster may enter the dead zone or exceed its physical limit, so that the control effect of the manipulator is poor, the preset task cannot be completed, and even the work is stopped. Based on this, the application designs a thrust allocation algorithm of sequential quadratic programming, and reconstructs the quadratic programming problem, the inequality constraint discontinuity problem caused by the thruster dead zone is integrated into the objective function in the form of a penalty function, thereby reducing the energy consumption of the underwater swimming manipulator, improving the dynamic allocation accuracy of the thruster thrust value, and improving the precise control of the manipulator in complex environment.

[0194] Please refer to Figure 2 , Figure 2 It is a model schematic diagram of the underwater swimming manipulator of the application embodiment, the tail link 1 in the underwater swimming manipulator is connected with the middle link 2 through the flexible joint 103, the tail link 1 includes the tail cabin 101 and the thruster cabin 102 connected with the tail cabin 101, the thruster 104, the thruster 105, the thruster 106 and the thruster 107 are arranged on the thruster cabin 102, the middle link 2 is connected with the head link 3 through the joint cabin 203, the middle link 2 includes the main control cabin 201, the thruster cabin 202, the thruster 204, the thruster 205, the thruster 206 and the thruster 207 are arranged on the thruster cabin 202, the head cabin 301 is arranged in the head link 3.

[0195] Please refer to Figures 3-7 , Figure 3 It is an equivalent model schematic diagram of the application embodiment, Figure 4 It is an angle change and disturbance schematic diagram of the application embodiment in the process of stabilizing the underwater swimming manipulator, Figure 5 It is a position angle change and error schematic diagram of the application embodiment in the whole process of simulating the underwater swimming manipulator, Figure 6 It is a control input and constraint upper and lower limit change schematic diagram generated by the model predictive controller of the application embodiment, Figure 7 It is a thrust schematic diagram of the thruster of the application embodiment, specifically, in order to ensure the effectiveness of the application, the underwater swimming manipulator stabilizing control and thrust allocation simulation experiment under complex environmental disturbance are carried out based on MATLAB, please continue to refer to Figure 5 , when the disturbance as shown in Figure 4 is applied and the joints of the underwater swimming manipulator change according to the angle as shown in Figure 4 , the system error can still be kept in a very small range. Please continue to refer to Figure 6As the angle of the underwater swimming manipulator and the output of the sliding mode controller change, the upper and lower bounds of the constraints of the model predictive controller also change, and the output of the model predictive controller always remains within the range of the constraints. For further reference Figure 7 The thrust of each propeller avoids the dead zone of the propeller and does not exceed its mechanical constraints, thus determining the effectiveness of the present invention.

[0196] So far, the technical solutions of the present application have been described in combination with the preferred embodiments shown in the drawings, but it is easy for those skilled in the art to understand that the protection scope of the present application is obviously not limited to these specific embodiments. Those skilled in the art can make equivalent changes or replacements to the related technical features without departing from the principles of the present application, and the technical solutions after these changes or replacements will all fall within the protection scope of the present application.

Claims

1. A method for stabilizing control and thrust distribution of an underwater mobile manipulator in a complex environment, characterized in that: include: Establishing the kinematic and dynamic models of the underwater swimming manipulator, and decoupling the kinematic and dynamic models into a double-integral linearized dynamic model; An inner generalized superhelical adaptive sliding mode controller is designed to compensate for the errors caused by model linearization and external disturbances. A model predictive controller is determined based on the double-integral linearized dynamic model, and the controller constraints are adjusted in real time according to the output of the inner sliding mode controller and the changes in the joint angles of the underwater swimming manipulator, so that the underwater swimming manipulator can obtain an optimal control sequence that satisfies the time-varying constraints. Based on the output of the model predictive controller, a sequential quadratic programming thrust allocation algorithm with a penalty function is designed to calculate the thrust of each thruster of the underwater swimming manipulator to achieve stabilization control of the underwater swimming manipulator in a complex underwater environment; The process of designing a sequential quadratic programming thrust allocation algorithm with a penalty function includes: Calculate the thrust distribution matrix based on the thruster configuration of the underwater swimming manipulator; Taylor expand the objective function of the original nonlinear constraint problem near the iteration point to obtain the description of the quadratic programming subproblem; Write the discontinuous inequality constraints into the description of the quadratic programming subproblem in the form of penalty functions; A description formula of the quadratic programming problem is determined based on the thrust allocation matrix and the description formula of the quadratic programming sub-problem.

2. The method for stabilizing control and thrust distribution of underwater mobile manipulators in complex environments according to claim 1 is characterized in that: The process of establishing the kinematic and dynamic models of the underwater swimming manipulator includes: Establish the world coordinate system and the link coordinate system of the underwater swimming manipulator based on the right-hand rule; Calculating the position of the connecting rod of the underwater swimming manipulator in the world coordinate system based on the rotation matrix of the connecting rod coordinate system relative to the world coordinate system; Determining the kinematic and dynamic models of the underwater swimming manipulator based on the position and posture of the connecting rod; The posture includes the position and posture of the connecting rod in the world coordinate system.

3. The method for stabilizing control and distributing thrust of an underwater manipulator under complex environments according to claim 2 is characterized in that: The process of calculating the pose of the connecting rod of the underwater swimming manipulator in the world coordinate system includes: Determine a rotation matrix and a position vector relative to a world coordinate system based on the link coordinate system; Determine a combination matrix of the rotation matrix and the position vector as the posture of the connecting rod; The position and orientation are determined based on the posture and the combination of the two motion modes of rotation and translation along the screw axis.

4. The method for stabilizing control and thrust distribution of underwater mobile manipulators in complex environments according to claim 1 is characterized in that: The process of decoupling the kinematic and dynamic models into a double-integral linearized dynamic model includes: Determine state variables; Adjusting the kinematic and dynamic models of the underwater swimming manipulator into state space expressions based on the state variables; Designing an inverse dynamics nonlinear compensation control law including control instructions based on the kinematic and dynamic models; The combination of the state space expression and the inverse dynamics nonlinear compensation control law is determined to be a double-integral linearized dynamic model.

5. The method for stabilizing control and thrust distribution of underwater mobile manipulators in complex environments according to claim 4 is characterized in that: The state variables include a position vector and a velocity vector.

6. The method for stabilizing control and thrust distribution of underwater mobile manipulators in complex environments according to claim 1 is characterized in that: The process of designing the inner generalized superhelical adaptive sliding mode controller includes: Determine the generalized super-helical sliding mode controller with adaptive gains; Adaptively adjusting control gains based on an update law to determine a comprehensive control performance of the controller; The generalized superhelical sliding mode controller with comprehensive control performance is determined to be the inner generalized superhelical adaptive sliding mode controller.

7. The method for stabilizing control and distributing thrust of an underwater manipulator under complex environments according to claim 1 is characterized in that: The process of determining the model predictive controller based on the double-integral linearized dynamics model includes: Determine the state, input and output of the underwater swimming manipulator; Constructing a state transfer equation of the underwater swimming manipulator based on the state quantity, input quantity and output quantity; A model predictive controller is determined based on the state transition equation.

8. The method for stabilizing control and distributing thrust of an underwater manipulator under complex environments according to claim 1 is characterized in that: The model predictive controller generates pseudo control instructions and requires real-time updating of constraints.

9. The method for stabilizing control and distributing thrust of an underwater mobile manipulator under complex environments according to claim 8 is characterized in that: The constraints include the system generalized inertia matrix, the limit value of the generalized force calculated from the configuration and mechanical parameters of the underwater robot propeller, the limit value of the torque calculated from the configuration and mechanical parameters of the underwater robot propeller, the output of the sliding mode controller, and the output of the model predictive controller.

Images