Method for stabilizing control and thrust distribution of underwater moving mechanical arm in complex environment

Abstract

The invention relates to the field of mechanical arm stabilization control, in particular to a stabilization control and thrust distribution method for an underwater swimming mechanical arm in a complex environment, which comprises the following steps of: establishing a kinematics model and a dynamics model of the underwater swimming mechanical arm, and decoupling the kinematics model and the dynamics model into a double-integral linearization model; designing an inner-layer generalized superhelix adaptive sliding mode controller to compensate errors and external disturbance caused by model linearization, and determining a model prediction controller based on the double-integral linearization dynamic model; and based on the output of the controller, a sequential quadratic programming thrust distribution algorithm with a penalty function is designed, and the thrust of each propeller of the underwater swimming mechanical arm is calculated. The method can effectively cope with control challenges of the underwater swimming mechanical arm in the aspects of hovering stabilization, propeller dead zones and the like, is suitable for numerous systems with strong coupling nonlinear characteristics and motion control systems with dead zones or physical constraints, and improves accurate control of the mechanical arm in a complex environment.

Description

Technical Field

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

[0002] With the development and utilization of marine resources in my country, underwater robots and other carriers for marine surveys and detection have also been increasingly widely used. As a new type of underwater robot, the underwater swimming manipulator adopts a multi-joint chain bionic snake-like design and is equipped with multiple thrusters. It has the characteristics of high degree of freedom, strong operability, flexible controllability, etc. It can adapt to the changing underwater environment and is good at performing operations in a small space. The hovering stabilization control of the underwater swimming manipulator is the basis for its realization of autonomous operation tasks. Through precise hovering control, the robot can remain stable in a specific position and perform long-term observation or operation tasks. Research on the hovering stabilization control technology of the underwater swimming manipulator will help improve its operating accuracy and stability in complex marine environments, which is crucial for delicate underwater operations.

[0003] For example, Chinese patent publication number: CN114714359A, discloses a saturation parameterized control method for a robotic arm system, which includes: establishing a dynamic model of the robotic arm system, designing a robotic arm system controller, actuator saturation linearization, system model conversion, introducing a parameterized matrix, and solving the system gain matrix. Based on the parameterized method, the invention takes into account the influence of actuator saturation and designs a controller suitable for the robotic arm system. By canceling the dynamic characteristics of the open-loop system, a system with the desired closed-loop characteristics is obtained, which avoids the damage to the full-drive characteristics of the system and realizes the stabilization control of the robotic arm system.

[0004] For example, Chinese Patent Publication No.: CN118456437A discloses a control method, a control device, an electronic device and a storage medium for a flexible robotic arm. The method constructs a non-singular integral terminal sliding mode surface based on a positive definite diagonal matrix and a trajectory tracking error, and has the characteristics of high robustness, high precision and fast transient response. Based on the dynamic model of the flexible robotic arm and combined with the non-singular integral terminal sliding mode surface, a backstepping hierarchical design strategy is adopted to construct a virtual control law layer by layer to obtain the input control torque of the flexible robotic arm, and the input control torque is used as the control instruction of the flexible robotic arm to adjust the flexible robotic arm to achieve tracking control of the desired trajectory, thereby ensuring that the sliding mode controller can converge to 0 within a finite time, and the convergence time can be controlled by parameters to a certain extent, meeting the rapid requirements of complex stabilization after capture, and maintaining a small steady-state error and torque jitter.

[0005] However, there are still the following problems in the prior art:

[0006] When the underwater mobile robot relies on thrusters to achieve its own posture changes during the execution of its operating tasks, the existence of the thruster operation dead zone may have an adverse effect on the underwater mobile robot's operating tasks, making the motion control of the underwater robot complicated and affecting its stability and control accuracy. Summary of the invention

[0007] To this end, the present invention provides a method for stabilizing control and thrust distribution of an underwater swimming manipulator in a complex environment, so as to solve the problem that when the underwater swimming manipulator relies on a thruster to achieve its own posture change during the execution of an operating task, the existence of a thruster operation dead zone may have an adverse effect on the operating task of the underwater swimming manipulator, making the motion control of the underwater robot complicated and affecting its stability and control accuracy.

[0008] To achieve the above object, the present invention provides a method for stabilizing control and thrust distribution of an underwater swimming manipulator in a complex environment, which comprises:

[0009] 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;

[0010] An inner generalized superhelical adaptive sliding mode controller is designed to compensate for the errors caused by model linearization and external disturbances.

[0011] 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 sliding mode controller and the change of the joint angle of the underwater swimming manipulator, so that the underwater swimming manipulator can obtain an optimal control sequence that satisfies the time-varying constraints;

[0012] 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.

[0013] Furthermore, the process of establishing the kinematic and dynamic models of the underwater swimming manipulator includes:

[0014] Establish the world coordinate system and the link coordinate system of the underwater swimming manipulator based on the right-hand rule;

[0015] Calculating the position and posture 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;

[0016] Determine the kinematic and dynamic models of the underwater swimming manipulator based on the position and posture of the connecting rod;

[0017] The position and posture include the position and posture of the connecting rod in the world coordinate system.

[0018] Further, the process of calculating the position and posture of the connecting rod of the underwater swimming robot arm in the world coordinate system includes:

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

[0020] Determine a combination matrix of the rotation matrix and the position vector as the posture of the connecting rod;

[0021] The position and orientation are determined based on the posture and the combination of two motion modes: rotation and translation along the screw axis.

[0022] Furthermore, the process of decoupling the kinematic and dynamic models into a double-integral linearized dynamic model includes:

[0023] Determine state variables;

[0024] Adjusting the kinematics and dynamics models of the underwater swimming manipulator into state space expressions based on the state variables;

[0025] Designing an inverse dynamics nonlinear compensation control law including control instructions based on the kinematic and dynamic models;

[0026] The combination of the state space expression and the inverse dynamics nonlinear compensation control law is determined to be a double integral linearized dynamics model.

[0027] Furthermore, the state variables include a position vector and a velocity vector.

[0028] Furthermore, the process of designing the inner generalized superhelical adaptive sliding mode controller includes:

[0029] Determine the generalized super-helical sliding mode controller with adaptive gains;

[0030] Adaptively adjusting control gains based on an update law to determine a comprehensive control performance of the controller;

[0031] The generalized super-helical sliding mode controller with comprehensive control performance is determined to be the inner generalized super-helical adaptive sliding mode controller.

[0032] Furthermore, the process of determining a model predictive controller based on the double-integral linearized dynamic model includes:

[0033] Determine the state quantity, input quantity and output quantity of the underwater swimming manipulator;

[0034] Constructing a state transfer equation of the underwater swimming manipulator based on the state quantity, input quantity and output quantity;

[0035] A model predictive controller is determined based on the state transfer equation.

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

[0037] Furthermore, 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 thruster, the limit value of the torque calculated from 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.

[0038] Furthermore, the process of designing a sequential quadratic programming thrust allocation algorithm with a penalty function includes:

[0039] Calculate the thrust distribution matrix based on the thruster configuration of the underwater swimming manipulator;

[0040] The objective function of the original nonlinear constraint problem is Taylor expanded near the iteration point to obtain the description of the quadratic programming subproblem;

[0041] Write discontinuous inequality constraints into the description of the quadratic programming subproblem in the form of penalty functions;

[0042] The 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.

[0043] Compared with the prior art, the present invention establishes the kinematic and dynamic models of the underwater swimming manipulator, decouples the kinematic and dynamic models into a double integral linearized dynamic model, designs an inner generalized superhelical adaptive sliding mode controller to compensate for model errors and external disturbances, and determines a model predictive controller based on the double integral linearized dynamic model to ensure that the controlled system 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; 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 its stabilization control in a complex underwater environment. The control scheme proposed by the present invention can effectively cope with the control challenges of the underwater swimming manipulator in terms of hovering stabilization and thruster dead zone, and is suitable for many systems with strongly coupled nonlinear characteristics and motion control systems with dead zones or physical constraints. It eliminates the chattering phenomenon of the controller to a certain extent, reduces the energy consumption of the underwater swimming manipulator, and improves the precise control capability of the manipulator in a complex environment.

[0044] In particular, the present invention decouples the dynamic model of the underwater swimming robotic arm into a double-integral linearization model. In the dynamic model, errors are easily accumulated and propagated between various degrees of freedom, resulting in a decrease in control accuracy. Through decoupling, errors can be effectively isolated and error accumulation can be reduced, thereby improving control accuracy. Based on this, the present invention decouples the dynamic model of the underwater swimming robotic arm into a double-integral linearization model, providing a theoretical basis for subsequent controller design. The decoupled model eliminates or weakens the coupling effect between various degrees of freedom, and can independently control each degree of freedom, avoiding adverse effects on other degrees of freedom when controlling one degree of freedom, thereby improving the stability and performance of the control system and improving the precise control of the robotic arm in a complex environment.

[0045] In particular, the present invention designs an inner-layer generalized superhelix adaptive sliding mode controller to compensate for the model uncertainty caused by unmodeled dynamics and linearization and the influence of complex environmental disturbances. When actually performing stabilization control on the robotic arm, most controllers are designed directly based on the linearized model without considering the influence of model uncertainty and environmental factors, which causes deviations in the calculation results of the controller and leads to instability of the robotic arm. Based on this, the present invention designs an inner-layer generalized superhelix adaptive sliding mode controller to compensate for the errors caused by inaccurate models and external disturbances, making the robotic arm more stable and improving the precise control of the robotic arm in a complex environment.

[0046] In particular, the present invention determines a model predictive controller based on a double integral linearization model, so that the upper and lower bounds of the constraints in the model predictive controller can be adjusted in real time according to the output of the pre-model predictive controller and the changes in the joint angles of the underwater swimming manipulator. In actual situations, the manipulator may be affected by external interference during operation, deviate from the predetermined target, and fail to complete the predetermined work task. Based on this, the present invention determines a model predictive controller, receives the state information of the underwater swimming manipulator and the output of the sliding mode controller in real time, and updates the boundaries of its output constraints, thereby ensuring that the manipulator achieves optimal control while satisfying the state and real-time input constraints, thereby improving the precise control of the manipulator in complex environments.

[0047] In particular, the present invention designs a sequential quadratic programming thrust allocation algorithm with a penalty function to prevent the output of the thruster from being in a dead zone or exceeding 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 are likely to enter the dead zone or exceed their physical limits, making the control effect of the robotic arm worse, failing to complete the preset task, or even stopping working. Based on this, the present invention designs a sequential quadratic programming thrust allocation algorithm and reconstructs the quadratic programming problem, integrating the problem of discontinuous inequality constraints caused by the thruster dead zone into the objective function in the form of a penalty function, thereby reducing the energy consumption of the underwater swimming robotic arm, allowing the thrust value of the thruster to always maintain the optimal solution, and improving the precise control of the robotic arm in a complex environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 A schematic diagram of the steps of a method for stabilizing and controlling an underwater swimming manipulator and distributing thrust in a complex environment according to an embodiment of the invention;

[0049] Figure 2 A schematic diagram of a model of an underwater mobile robotic arm according to an embodiment of the invention;

[0050] Figure 3 It is a schematic diagram of an equivalent model of an embodiment of the invention;

[0051] Figure 4 A schematic diagram of angle changes and disturbances during the stabilization process of an underwater mobile manipulator according to an embodiment of the invention;

[0052] Figure 5 A schematic diagram of the position angle change and error of the underwater swimming robot arm during the whole simulation process of an embodiment of the invention;

[0053] Figure 6 A schematic diagram of the control input and upper and lower bounds of constraints generated by the model predictive controller of an embodiment of the invention;

[0054] Figure 7 A thrust diagram of a propeller according to an embodiment of the invention;

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

[0056] 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.

[0057] 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.

[0058] See also Figure 1 , Figure 1 A schematic diagram of the steps of a method for stabilizing and controlling a manipulator underwater under a complex environment and distributing thrust in accordance with an embodiment of the invention. A method for stabilizing and controlling a manipulator underwater under a complex environment and distributing thrust in accordance with the invention comprises:

[0059] 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;

[0060] An inner generalized superhelical 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 sliding mode controller and the change of the joint angle of the underwater swimming manipulator, so that the underwater swimming manipulator can obtain an optimal control sequence that satisfies the time-varying constraints;

[0062] 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.

[0063] Specifically, the links of the underwater swimming robot arm are numbered as link 1, link 2, ..., link n from the tail to the head, where 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 kinematic and dynamic models of the underwater swimming manipulator includes:

[0065] Establish the world coordinate system and the link coordinate system of the underwater swimming manipulator based on the right-hand rule;

[0066] Calculating the position and posture 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;

[0067] Determine the kinematic and dynamic models of the underwater swimming manipulator based on the position and posture of the connecting rod;

[0068] The position and posture include the position and posture of the connecting rod in the world coordinate system.

[0069] Specifically, establishing a coordinate system based on the right-hand rule is an existing technology, and it is only necessary to ensure that the coordinate system is successfully established. Those skilled in the art can establish the coordinate system according to actual conditions, which will not be described in detail.

[0070] Specifically, the kinematic and dynamic models of the underwater swimming manipulator are expressed by formula (1):

[0071]

[0072] In formula (1), M(ξ) represents the generalized inertia matrix of the system, abbreviated as M, N represents the vector of factors such as model uncertainty and external disturbance, τ represents the generalized force and torque of the manipulator, ζ represents the velocity vector of the underwater mobile manipulator in the base coordinate system, and ξ is expressed by formula (2):

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

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

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

[0076] Specifically, the process of calculating the position and posture of the connecting rod of the underwater swimming robot arm in the world coordinate system includes:

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

[0078] Determine a combination matrix of the rotation matrix and the position vector as the posture of the connecting rod;

[0079] The position and orientation are determined based on the posture and the combination of two motion modes: rotation and translation along the screw axis.

[0080] Specifically, the posture of the connecting rod is expressed by formula (3):

[0081]

[0082] In formula (3), represents the rotation matrix of the link coordinate system relative to the world coordinate system, represents the position vector,

[0083] Specifically, the infinitesimal representation method in spiral motion is used to describe the instantaneous linear velocity and angular velocity of the connecting rod with the spinor, and the spinor of joint i is As a skew-symmetric matrix The generalization of , the spinor is expressed by formula (4),

[0084]

[0085] In formula (4), represents the linear velocity vector of joint i, represents the angular velocity vector of joint i, express The skew-symmetric matrix of , se(3) represents the Lie algebra of the special Euclidean group.

[0086] Specifically, the position and posture of each link are expressed by formula (5):

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

[0088] In formula (5), θ i represents the angle of the i-th joint, A i (θ i ) represents the combination of the two motion modes of the i-th connecting rod, rotation and translation along the screw axis.

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

[0090]

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

[0092] Specifically, the axis of the motion spin is the spin axis, and the motion spin is expressed by formula (9):

[0093]

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

[0095] Specifically, the Jacobian matrix J i Formula (10) describes the relationship between the generalized velocity in the base coordinate system of the underwater mobile manipulator and the motion rotation of each link in the link coordinate system.

[0096]

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

[0098] Specifically, the Jacobian matrix J i It is expressed 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 Ad- 1 (Aj,i) specific manifestation, Ad- 1 (Aj,i) represents the joint adjoint operator, represents the spinor coordinates of joint i, and the adjoint operator Ad -1 (A i ) and the joint adjoint operator Ad -1 (A j,i ) is expressed 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 the rotation matrix of link i relative to the link j coordinate system, Represents the position vector of link i relative to the link j coordinate system.

[0104] Specifically, the process of decoupling the kinematic and dynamic models into a double-integral linearized dynamic model includes:

[0105] Determine state variables;

[0106] Adjusting the kinematics and dynamics models of the underwater swimming manipulator into state space expressions based on the state variables;

[0107] Designing an inverse dynamics nonlinear compensation control law including control instructions based on the kinematic and dynamic models;

[0108] The combination of the state space expression and the inverse dynamics nonlinear compensation control law is determined to be a double integral linearized dynamics model.

[0109] Specifically, the state variables include a position vector and a velocity vector.

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

[0111]

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

[0113] Specifically, based on the state variables, the kinematic and dynamic models of the underwater swimming manipulator are adjusted to the state space expression represented by formula (15):

[0114]

[0115] In formula (15), The state space expression for the position vector in the base coordinate system is, State-space expression for the velocity vector in base coordinates.

[0116] It can be understood that the inverse dynamics nonlinear compensation control law including control instructions designed based on the kinematic and dynamic models is expressed by formula (16):

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

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

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

[0120] Specifically, the present invention decouples the dynamic model of the underwater swimming robotic arm into a double-integral linearization model. In the dynamic model, errors are easily accumulated and propagated between various degrees of freedom, resulting in a decrease in control accuracy. Through decoupling, errors can be effectively isolated and error accumulation can be reduced, thereby improving control accuracy. Based on this, the present invention decouples the dynamic model of the underwater swimming robotic arm into a double-integral linearization model, providing a theoretical basis for subsequent controller design. The decoupled model eliminates or weakens the coupling effect between various degrees of freedom, and can independently control each degree of freedom to avoid adverse effects on other degrees of freedom when controlling one degree of freedom, thereby improving the stability and performance of the control system and improving the precise control of the robotic arm in a complex environment.

[0121] Specifically, the process of designing the inner generalized superhelical adaptive sliding mode controller includes:

[0122] Determine the generalized super-helical sliding mode controller with adaptive gains;

[0123] Adaptively adjusting control gains based on an update law to determine a comprehensive control performance of the controller;

[0124] The generalized super-helical sliding mode controller with comprehensive control performance is determined to be the inner generalized super-helical adaptive sliding mode controller.

[0125] Specifically, the generalized super-helical sliding mode controller with adaptive gain is expressed by equation (17),

[0126]

[0127] In formula (17), σ represents the state error of the robot arm, k1 represents the first gain of the controller, k2 represents the second gain of the controller, β represents the third gain of the controller, and η represents a continuous but non-smooth function.

[0128] Specifically, when the system moves on the sliding surface, σ is approximately 0, which weakens the step characteristics of the sign function. η plays a role in suppressing disturbances, keeping the system robust and enabling the system to converge within a finite time in the presence of disturbances. The sign function in η is hidden in its differential term, ensuring the continuity of the output and weakening the jitter of the system. The value of k1 determines the comprehensive control performance of the system, such as the convergence time of the system under ideal conditions. The value of k2 determines the robustness and anti-disturbance ability of the robot arm.

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

[0130]

[0131] In formula (18), ε represents a first normal number, λ represents a second normal number, γ1 represents a third normal number, and ω1 represents a fourth normal number.

[0132] Specifically, the present invention designs an inner generalized superhelix adaptive sliding mode controller to compensate for the model uncertainty caused by unmodeled dynamics and linearization and the influence of complex environmental disturbances. When actually performing stabilization control on the robotic arm, most of the controllers are designed directly based on the linearized model without considering the influence of model uncertainty and environmental factors, which causes deviations in the calculation results of the controller and leads to instability of the robotic arm. Based on this, the present invention designs an inner generalized superhelix adaptive sliding mode controller to compensate for the errors caused by the model and the errors caused by external disturbances, making the robotic arm more stable and improving the precise control of the robotic arm in a complex environment.

[0133] Specifically, the process of determining the model predictive controller based on the double-integral linearized dynamic model includes:

[0134] Determine the state quantity, input quantity and output quantity of the underwater swimming manipulator;

[0135] Constructing a state transfer equation of the underwater swimming manipulator based on the state quantity, input quantity and output quantity;

[0136] A model predictive controller is determined based on the state transfer equation.

[0137] Specifically, the state quantity of the robot arm is expressed by formula (19):

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

[0139] In formula (19), x1=x, x2=y, x3=z, x4=φ, x5=θ, x6=ψ.

[0140] The input of the robot arm is expressed by formula (20):

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

[0142] In formula (20), u1=x, u2=y, u3=z, u4=φ, u5=θ, u6=ψ.

[0143] The output of the robot arm is expressed by formula (21):

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

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

[0146] Specifically, the state transfer equation is expressed by formula (22):

[0147]

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

[0149]

[0150] The definitions of the various vectors generated during the prediction process are expressed by formula (24):

[0151]

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

[0153] Specifically, the linear model predictive control algorithm is transformed 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 cost function of the linear prediction output equation, τ min represents the lower limit of the generalized force of the underwater swimming manipulator, τ max Represents the upper limit of 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] The cost function is expressed by formula (27):

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

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

[0162] Specifically, the present invention designs a model predictive controller based on a 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 changes in the joint angles of the underwater swimming robotic arm. In actual situations, the robotic arm may be affected by external interference during operation, deviate from the preset target, and fail to complete the preset task. Based on this, the present invention designs a model predictive controller to receive the state information of the underwater swimming robotic arm and the output of the sliding mode controller in real time, and update the boundaries of its output constraints, thereby ensuring that the robotic arm achieves optimal control while satisfying the state and real-time input constraints, thereby improving the precise control of the robotic arm in complex environments.

[0163] Specifically, the model predictive controller generates pseudo control instructions, which require real-time updating of constraints.

[0164] Specifically, the constraints include the system generalized inertia matrix, 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 is expressed 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 will be updated in real time as the joint angles of the underwater swimming robotic arm change.

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

[0169] Calculate the thrust distribution matrix based on the thruster configuration of the underwater swimming manipulator;

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

[0171] Write discontinuous inequality constraints into the description of the quadratic programming subproblem in the form of penalty functions;

[0172] The 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.

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

[0174]

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

[0176] The thrust distribution of the underwater mobile manipulator is expressed by formula (30):

[0177] τ=Tu (30)

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

[0179] The thrust distribution matrix is ​​expressed by formula (31):

[0180]

[0181] In formula (31), represents the Jacobian matrix of the connecting rod of the first group of thruster modules, represents the Jacobian matrix of the connecting rod where the second set of thruster modules are located, Β joint It is expressed by formulas (32)-(34):

[0182]

[0183] In formulas (32)-(34), γ βi,j represents the thrust point of the j-th thruster on the connecting rod i in the connecting rod coordinate system, β βi,j represents the thrust direction of the j-th thruster on connecting rod i in the connecting rod coordinate system,

[0184] Specifically, the general nonlinear programming problem is expressed by formula (35),

[0185]

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

[0187] Specifically, the objective function of the original nonlinear constraint problem is transformed into k Taylor expansion can be performed near , which can be approximated as a linear function, and then the quadratic programming subproblem is expressed 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 the implementation, the discontinuous inequality constraints are written into the quadratic programming subproblem in the form of penalty functions. The description of the quadratic programming problem is obtained by combining the thrust allocation matrix and the description of the quadratic programming subproblem, which is expressed by formula (37):

[0191]

[0192] In formula (37), u i represents the thrust of each thruster, u min represents the lower limit of thrust of the propeller, u max represents the upper limit of thrust of the propeller, C represents the penalty coefficient, C = 1×10 6 , u represents the initial value of the quadratic programming problem, represents the pseudo-inverse of the thrust allocation matrix T.

[0193] Specifically, the present invention designs a sequential quadratic programming thrust allocation algorithm with a penalty function to avoid the output of the thruster being in a dead zone or exceeding 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 are likely to enter the dead zone or exceed their physical limits, making the control effect of the robotic arm worse, failing to complete the preset task, or even stopping working. Based on this, the present invention designs a sequential quadratic programming thrust allocation algorithm and reconstructs the quadratic programming problem, integrating the problem of discontinuous inequality constraints caused by the thruster dead zone into the objective function in the form of a penalty function, thereby reducing the energy consumption of the underwater swimming robotic arm, improving the accuracy of the dynamic allocation of the thrust value of the thruster, and improving the precise control of the robotic arm in a complex environment.

[0194] See also Figure 2 , Figure 2 This is a model schematic diagram of an underwater swimming manipulator according to an embodiment of the invention, in which a tail link 1 is connected to an intermediate link 2 via a flexible joint 103, the tail link 1 comprises a tail cabin 101 and a thruster cabin 102 connected to the tail cabin 101, the thrusters 104, 105, 106 and 107 are arranged on the thruster cabin 102, the intermediate link 2 is connected to a head link 3 via a joint cabin 203, the intermediate link 2 comprises a main control cabin 201 and a thruster cabin 202, the thrusters 204, 205, 206 and 207 are arranged on the thruster cabin 202, and a head cabin 301 is arranged in the head link 3.

[0195] See also Figure 3-Figure 7 , Figure 3 is a schematic diagram of an equivalent model of an embodiment of the invention, Figure 4 This is a schematic diagram of the angle change and disturbance during the stabilization process of the underwater swimming manipulator of the embodiment of the invention. Figure 5 This is a schematic diagram of the position angle change and error of the underwater swimming robot arm during the whole simulation process of the embodiment of the invention. Figure 6 This is a schematic diagram of the control input and upper and lower bounds of constraints generated by the model predictive controller of an embodiment of the invention. Figure 7 The thrust diagram of the propeller of the embodiment of the invention is shown in FIG. Specifically, in order to ensure the effectiveness of the invention, a simulation experiment of underwater swimming manipulator stabilization control and thrust distribution under complex environmental disturbance is carried out based on MATLAB. Figure 5 , when applying Figure 4 The perturbation shown here controls the joints of the underwater swimming robot according to Figure 4 When the angle shown is changed, the system error can still be kept within a small range. Figure 6, when the angle of the underwater 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 accordingly, and the output of the model predictive controller always remains within the constraint range. Figure 7 , the thrust of each thruster avoids the thruster dead zone and does not exceed its mechanical constraints, thereby confirming the effectiveness of the present invention.

[0196] 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 stabilizing control and thrust distribution of an underwater mobile robot arm 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 dynamics model, and the controller constraints are adjusted in real time according to the output of the inner sliding mode controller and the change of the joint angle 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.

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 kinematics and dynamics model 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 and posture 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; Determine the kinematic and dynamic models of the underwater swimming manipulator based on the position and posture of the connecting rod; The position and posture include the position and posture of the connecting rod in the world coordinate system.

3. The method for stabilizing control and thrust distribution of underwater mobile manipulators in complex environments according to claim 2 is characterized in that: The process of calculating the position and posture of the connecting rod of the underwater swimming robot arm 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 two motion modes: 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-integrated linearized dynamic model includes: Determine the state variables; Adjusting the kinematics and dynamics 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 dynamics 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 super-helical sliding mode controller with comprehensive control performance is determined to be the inner generalized super-helical adaptive sliding mode controller.

7. 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 determining the model predictive controller based on the double-integral linearized dynamics model includes: Determine the state quantity, input quantity and output quantity 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 transfer equation.

8. The method for stabilizing control and thrust distribution of underwater mobile manipulators in complex environments according to claim 1 is characterized in that: The model predictive controller generates pseudo control instructions, and the constraints need to be updated in real time.

9. The method for stabilizing control and thrust distribution of underwater mobile manipulators in 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 thruster, the limit value of the torque calculated from 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.

10. The method for stabilizing control and thrust distribution of underwater mobile manipulators in complex environments according to claim 1, characterized in that: 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; The objective function of the original nonlinear constraint problem is Taylor expanded near the iteration point to obtain the description of the quadratic programming subproblem; Write 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.

Images